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ABSTRACT 


This thesis develops the mathematical relationships necessary to implement 
alternating tripod gaits on the hexapod underwater walking machine, AquaRobot. Analysis 
of documentation and application of Denavit-Hartenberg kinematic modeling techniques 
determine the fundamental vehicle parameters. Smooth leg motion models following 
elliptical and cycloidal trajectories are devised. Gait planning algorithms, using the 
elliptical smooth leg motion model, are developeu for both discrete and continuous body 
motion. Statically stable, alternating tripod gait simulations are implemented in the 
programming language. A stick figure graphics display allows examination and testing of 
the gait algorithms prior to incorporation in follow-on 3D graphics simulations or in real¬ 
time operation. 
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I. INTRODUCTION 


Most of us are familiar with the development and widespread use of wheeled 
locomotion. Prepared surfaces, in the form of roadways and railways, provide the means 
for wheeled vehicles to efficiently transport people and property over long distances. 
Wheeled vehicles, however, are significantly less efficient over irregular, unprepared 
terrain. Even tracked vehicles, whose tracks are simply elongated wheels, have difficulty 
traversing irregular terrain. Wheeled and tracked vehicles, then, require special terrain. 

Legged locomotion, on the other hand, offers several potential advantages over 
wheels or tracks when traversing natural terrain. First, and foremost, legs allow the 
flexibility to select and place supporting feet. Flexible foot placement provides 
opportunities to test the terrain prior to attempting vehicle support and extends vehicle 
mobility beyond prepared surfaces. Legged vehicles can move on more general terrain. 

Another advantage is enhanced traction in pliable soil. Wheeled and tracked vehicles, 
as an inherent part of their design, make a depression in any non-supporting surface and 
then constantly try to climb out of that depression. Legged vehicles typically generate 
distinct footprints and use any material pushed up behind the foot to improve traction. 
[MCG85] 

Additional potential advantages of legged locomotion over wheeled or tracked 
locomotion include greater speed, less power consumption, and relatively small footprint. 
Finally, adoption of legged locomotion offers the unique advantage of integrating the 
locomotion function with a terrain measurement function. Specifically, legged locomotion 
allows simultaneous vehicle support, propulsion, and measurement of terrain height or 
depth. 
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Legged locomotion advantages are not lost in an underwater environment. Of the 
vehicles capable of submersible operation, 90 percent are of the tethered or untethered 
free swimming type. Although these vehicles effectively use their three-dimensional 
maneuverability, they have difficulty generating significant forces and have trouble 
maintaining stationary position and direction. The underwater wheeled or tracked 
(crawling) vehicles have poor maneuverability, but are capable of exerting very large 
working forces. As in the non-aquatic case, underwater wheeled or tracked locomotion is 
limited to relatively hard and flat sea bottom. Additionally, wheeled or crawling vehicles 
make the water so muddy that optical viewing devices or television cameras are rendered 
useless. Even in an underwater environment, legged locomotion overcomes many of the 
disadvantages of wheeled, tracked, or free swimming type locomotion. Legged vehicles 
can walk on uneven terrain with minimal disturbance of the surface, can provide a 
maneuverable, yet stable, platform for observation purposes, and can use appendages for 
accurate measurement. [ISH83] 

A. AQUAROBOT UNDERWATER WALKmG MACHINE 

In the early 1980's, the Japanese Port and Harbor Research Institute (PHRI) was 
tasked with finding an alternative method of carrying out inspection of underwater 
construction sites. They determined a robot was needed because of increased risks to and 
lower working efficiency of port construction workers at deeper sea areas and also 
because of a shortage of divers. Additionally, the PHRI decided the ten centimeter 
horizontal and five centimeter vertical accuracy requirements and average working depth 
of 50 meters were best achieved by mechanical means. 

TheAquaRobot project started in 1984. AquaRobot was originally designed with two 
primary functions: 1) measurement of the flatness of rock foundation mounds for tsunami 
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breakwaters using the motion of its six walking legs, and 2) observation and supervision of 
underwater construction using its on-board television camera. An experimental model and 
a prototype model were constructed. The experimental model was developed for above 
water research and testing. Based on the results of testing the experimental model, a 
watertight prototype model was subsequently developed. 

AquaRobot (Figure 1-1) is a six-legged, “insect type" robot. The body is hexagonal 
and the legs are installed on the sides of the hexagonal frame. Each of its six legs has three 
articulated joints driven by DC motors. Each leg includes a touch sensor on the foot. 
AquaRobot is currently controlled by a 16 bit 80286-based microcomputer. Additional 
geometrical detail of AquaRobot is provided in Chapter III. A more complete overview 
and history is found in Appendix A. [AKI89] 

B. MOTIVATION FOR AQUAROBOT GAFT PLANNING 

This thesis is a direct outgrowth of a cooperative research effort between the Naval 
Postgraduate School (NFS) and the Japanese PHRI to enhance the hardware and software 
capabilities of the AquaRobot underwater walking robot. The National Science 
Foundation (NSF) is providing support to the NPS, while the Japanese Science and 
Technology Agency (STA) is supporting the PHRI. 

The PHRI originally developed AquaRobot to replace hard-hat divers constructing 
tsunami barriers in the hazardous underwater environment off Japan's coast. Although 
AquaRobot has demonstrated significant ability as the first hexapod underwater walking 
machine, it is still unable to perform its designed task more efficiently or less costly than 
human divers. AquaRobot speed and agility enhancements derived from gait planning 
algorithms in this thesis Avill result in a more efficient and less costly machine. This 
translates directly into reduced risk of human injury and decreased construction costs. 
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Figure 1-1. The Hexapod Underwater Walking Machine AquaRobot. 
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Although walking machine research and construction is extensive, AquaRobot is the 
first walking robot with a practical use. As such, improvements to AquaRobot directly 
result in concept fulfillment and favorably demonstrate the usefulness of robots, and 
walking robots in particular. 

C. LITERATURE SURVEY 

Obviously, the most reviewed literature was that generated at the PHRI by Akizono 
[AKI89] and Iwasaki [IWA87], [IWA88a], [IWA88b], and [IWA90]. These papers 
provided AquaRobot-sp&o&c development and implementation information. Craig's 
textbook [CRA86] yielded the fundamental robotics techniques for solutions of 
kinematics, inverse kinematics, coordinate transformations, and workspace problems. 

Some information found in Hirose's seminal series of papers on gait algorithms for 
quadruped vehicles [HIR84], [HIR86a], [HIR86b], and [HIR88] proved transferable to 
hexapod vehicles. Two of McGee's papers [MCG85], [MCG79] were consulted for their 
insight into hexapod walking issues. 

Although several walking robot textbooks were reviewed, specific walking theory 
was best presented in Song's textbook [SON89], while general walking theory was 
covered quite well in Toad'-' textbook [TOD853. 

Lyman's thesis [LYM87], along with papers by Kwak [KWA90], Lee [LEE88a] and 
[LEE88b], and Waldron [WAL84] pro'/ided analysis of gait control for the Adaptive 
Suspension Vehicle, a hexapod walking machine Avith bilateral symmetry. 

Davidson's [DAV93] and Grim's [GRI93] theses provided baseline graphics code 
development information. Kanayama's manuscript on spatial reasoning fKAN92] proved 
the best source for efficient motion planning algorithms. 
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D. THESIS ORGANIZATION 


The final part of this chapter is devoted to outlining the thesis organization. Chapter 
II provides the setting for the remainder of the thesis. A brief history of walking machines 
is presented, along with a survey of previous work and an introduction to walking theory. 

A physical description of the AquaRobot underwater walking robot is the emphasis of 
Chapter III. Geometrical features and system parameters are defined. Some aspects of 
AquaRobot are idealized for simplification. A detailed problem statement is included in 
this chapter. 

Existing AquaRobot documentation is sparse and most detailed information is written 
in Japanese. Therefore, Chapter IV develops fundamental vehicle parameters through 
analysis of written reports, review of construction blueprints, and application of Denavit- 
Hartenberg kinematic modeling techniques. Inverse kinematic equations and relations are 
presented and necessary coordinate transformations are developed. Workspace volumes 
are determined, appropriate workspace constraints identified, and the strategy for 
workspace use is presented. 

Chapter V introduces the stability model used throughout this thesis. AquaRobot's 
center-of-body and center-of-gravity relations are defined. The issue of static versus 
dynamic stability is addressed. 

AquaRobot currently uses rectangular leg motions for all gaits. Curved leg motions 
result in smoother gaits. In Chapter VI, two smooth leg motion models are developed. 
Elliptical and cycloidal leg motion curves are generated for use in the AquaRobot 
simulator. 

Statically stable tripod gait are planned and implemented in Chapter VII. To ease 
initial gait implementation, flat terrain, fixed body height, fixed body orientation, and 
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straight line path constraints are placed on the vehicle. Some constraints are lifted as the 
thesis progresses. Ultimately, a continuous omnidirectional tripod gait is developed. 

Chapter VIII presents the AquaRobot simulation program. The simulation features 
and program organization are described. The final chapter summarizes the contributions of 
this thesis. Future research areas are also outlined. Appendix A contains an overview and 
history of the AquaRobot. Appendices B and C contain program code. Appendix D is an 
AquaRobot Simulator User's Manual that is separable fi'om the thesis. 
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n. SURVEY OF PREVIOUS WORK 


Three aspects of practical walking machine design are considered crucial; control of 
legged vehicles, actuator and leg design, and gait study. Vehicle control is considered the 
most crucial because of its inherent complexity. Many researchers are currently devoting 
extensive efforts to this research area. Only a few walking machines have sophisticated leg 
designs. Serious study has only recently been given to improving leg efficiency through 
better designs of legs and actuators. Finally, previous gait study has been concentrated on 
straight line motion over flat terrain. This thesis provides the foundation to extend gait 
study 'm.AquaRobot beyond these motion and terrain constraints. [TOD8S] 

A A BRIEF HISTORY OF WALKING MACHINES 

Walking machines have been constructed with from one to eight legs, and possibly 
more. An even number of legs is almost universal as this allows efficient gaits for 
progression in a straight line. More legs were typically used in the past for heavily loaded, 
but slower vehicles, while bipeds and quadrupeds were generally faster and more agile. Sue 
legs is a magic number because it allows two alternating tripods. Two legs is another 
popular number because it allows emulation of human walking. Some of the many 
properties to consider when selecting the number of legs are: 1) stability, 2) energetic 
efficiency, 3) redundancy (using fewer legs to walk if some are unavailable), 4) joint 
control requirements, 5) cost, 6) weight, 7) desired complexity of sensing, and 8) possible 
gaits. [TOD85] 
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Two of the earliest examples of inventors' attempts to design walking machines are 
Rygg's mechanical horse in 1893 and Bechtolsheim's four-legged machine in 1913 There 
is no indication that either machine actually walked. [SON89] 

In the mid 1950’s, numerous research groups began to systematically study walking 
machines [SON89]. Then, in the 1960's, the Space General Corporation designed and 
constructed a six-legged machine and an eight-legged machine to investigate their 
applicability as lunar explorers. These two machines met their design goals, but exhibited 
poor terrain adaptability because of too few degrees of freedom. [TOD85] 

In 1966, Frank and McGhee developed the first autonomous legged vehicle to walk 
under computer control. They called their walking machine the "Phoney Pony". This four- 
legged machine was powered externally via a cable and only walked in a straight line. In 
1968, the General Electric Corporation built another quadruped, a 3000 pound vehicle 
with 12 degrees of freedom and a human rider-operator. The General Electric quadruped 
was successful at obstacle climbing and possessed good mobility in difficult terrain. 
Because its operation was highly complex and demanding, however, only a few people 
ever learned how to operate it. The General Electric quadruped proved the necessity for 
computer control of multi-degree-of-freedom vehicles. [TOD85] 

In the 1970's, several walking machines were constructed. A pneumatically powered 
and analog computer controlled biped was successfully operated in Yugoslavia. A series 
of bipeds was developed in Japan, with walking speeds ranging from 90 seconds per step 
to the speed of a slow human walk. From 1974 through 1979, Russia developed and 
operated two six-legged walking machines of the insect body type. Finally, in 1977, 
McGhee and his associates at the Ohio State University (OSU) first got the OSU Hexapod 
to walk. [TOD85] 
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With the progress in microcomputer technology, computer-aided design, and 
computer simulation, the 1980's ushered in the development of several new walking 
machines. In 1980, Hirose and Umetani, at the Tokyo Institute of Technology, built the 
four-legged Perambulating Vehicle II (PVII). The PVII was promising because of its 
sophisticated pantograph leg design. Hirose, in 1984, constructed an enlarged version of 
the PVII, called TITAN III. In 1982, the Ohio State University developed monopod and 
six-legged walking machines as part of the Adaptive Suspension Vehicle (ASV) project. 
[TOD85] 

More recently. Brooks, at the Massachusetts Institute of Technology, has designed 
and built several insectoid walking robots, one named Atilla, to demonstrate his theories 
on subsumption. Additionally, the Camaegie-Mellon University is working on a walking 
machine, the Ambler, designed to explore the surface of Mars. Finally, research continues 
on AquaRobot at the Naval Postgraduate School and the Japanese PHRI. 

B. SURVEY OF HEXAPOD VEHICLES 

Many hexapod vehicles have oeen designed and built to advance research in walking 
control, leg and actuator design, and gait planning. Six legs is a good compromise 
between speed (shown in the next section), stability (generally improved \vith a larger 
number of legs), and simplicity [WAL84]. The incremental stability gain is smaller when 
going from six legs to eight legs than when going from four legs to six legs [WAL84]. So, 
although there is a marked advantage to using hexapod vehicles over quadruped vehicles, 
the increasing mechanical and computational complexity of eight-legged vehicles offsets 
their minimal stability advantages. 

One of the first successful hexapod walking machines was that constructed at the 
University of Rome in 1972. This machine was simply a six-legged version of the Phoney 
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Pony. Then, as mentioned in the previous section, the OSU Hexapod walked in 1977 In 
1983, Odetics, Incorporated constructed the ODEX I, a six-legged walking machine with 
some climbing ability. ODEX II, also a hexapod machine, has since been designed for use 
in nuclear power plants [ODE86]. In 1985, the design and construction of a large, man¬ 
carrying hexapod called the Adaptive Suspension Vehicle was completed at OSU 
[MCG86]. The ASV is a self-contained walking machine designed to traverse natural 
terrain. Each of the hexapods discussed has made a great contribution to the study of 
walking control, leg and actuator design, and gait planning. However, of all the known 
hexapods, only AquaRobot has moved beyond the research phase into practical use 

C. INTRODUCTION TO WALKING THEORY 

The basic definitions and theorems for hexapod tripod gait planning used in this paper 
are introduced in this section. A gait is a method of locomotion distinguished by a specific 
pattern of lifting and placing of the feet. Gaits are often described using McGhee’s and 
Jain's event sequence notation [MCG72]. In such a notation, the integer i corresponds to 
the event of placing foot i on the ground. Integer i+n, where n equals the number of legs, 
represents lifting the same foot. AquaRobot's legs are numbered sequentially in a 
clockwise direction from one t'nrough six. 

The transfer phase of a leg is that time period when the foot is not touching the 
ground. The transfer phase is also known as the recovery time t or the return stroke 
[TOD85]. The support phase L of a leg, then, is simply the opposite of the transfer phase, 
the time period when the foot is touching the ground. The cycle time T is the time required 
for a complete cycle of leg locomotion of a periodic gait. The cycle time includes the 
transfer and support phases of the leg. A periodic gait is one in which every limb operates 
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with the same cycle time. Otherwise, the gait is non-periodic The duty factor is the 
fraction of the cycle time in which leg i is in the support phase. [SON89] Thus, 




time of support phase of leg i _ t^ 
cycle time of leg i f 


( 2 . 1 ) 


A periodic gait is singular if there is a simultaneous occurrence of two or more 
events during a locomotion cycle. Conversely, a periodic gait is nonsingular if no two of 
its events occur simultaneously [MCG68a]. A gait is considered regular if all legs have the 
same duty factor, as when 




U.j^l,2,...,n 
[w is the leg number 


( 2 . 2 ) 


The leg stride is a complete cycle of leg movements from a particular leg movement 
to the next occurrence of the same leg movement [TOD85]. The stride frequency /is the 
number of strides in unit time [TOD85]. The stride length X is the distance the center of 
gravity of the body travels during one stride [TOD85]. The leg stroke R is the distance the 
foot travels, relative to the body, during the support phase [SON89]. A step is defined as 
the interval from the time a leg is placed until the time the next leg is placed [HIR84]. 

A working volume is associated with each leg. This volume is a subset of the three- 
dimensional space, defined relative to the body, consisting of the points which the foot can 
reach [KWA90]. The support pattern of a walking machine is defined as "the two 
dimensional point set in a horizontal plane consisting of the convex hull of the vertical 
projection of all foot points in the support phase" [SON89]. Generally, the contact point 
between the foot and the ground is idealized to have no slip. Further, although the actual 
foot contact may be distributed over some small surface area, the center of pressure may 
be chosen as the foot contact point. A foothold is described as a point on a piece of terrain 
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[KWA90]. When a foot is placed on the terrain, its foothold becomes the support point of 
the leg [KVVA90]. A foothold can be assigned to a leg while it is still in the transfer phase 

There are four important definitions related to stability. First, stability margin for 
an arbitrary support pattern, is the shortest distance from the vertical projection of the 
center of gravity to any point on the boundary of the support pattern in the horizontal 
plane [KWA90]. Then, the front stability margin and the rear stability margin describe 
distances from the vertical projection of the center of gravity to the forward and rearward 
boundaries of the support pattern, respectively [SON89]. The front and rear stability 
margins are measured along the direction of motion. Finally, the longitudinal stability 
margin S, is defined as the shortest distance from the body’s center of gravity to the 
boundary of the support pattern, measured in the direction of travel [MCG86]. From the 
definition of longitudinal stability margin, a gait is deemed statically stable if .S; > 0. 
Otherwise, it is deemed statically unstable. 

With some fundamental gait definitions explained, we can now show that vehicle 
speed increases as the number of legs increases. Waldron et al. [WAL84] and Todd 
[TOD85] agree that the major limitation on speed of legged locomotion is the time 
required to return the leg through the air to its starting position. For any legged vehicle, 

there is a minimum time for a leg to be in this transfer phase. Additionally, there is a 
minimum duty factor for statically stable systems; quadrupeds, 

for hexapods, and for eight legs. Finally, if the leg stroke R 

return stroke t and cycle time T are presumed constant, the duty factor (3 causes the speed 
I' to change as 


R 





From previous definitions, 


^ ^support phase ^tranrfer phase ^s 


+ T. 


(2.4) 


Then, using equation 2.1 

Combining equations 2.4 and 2.5 

T=Tfi+T. 


Then, for a fixed t, 


T = 


\-P 


Finally, combining equations 2.3 and 2.7 yields 



tl] 

P ) 


Substituting into equation 2.8 yields 



Using Phesapod equation 2.8 yields 



T 


Lastly, using Peigu-ugs equation 2.8 provides 



(2.5) 

( 2 . 6 ) 

(2.7) 

( 2 . 8 ) 

(2.9) 

( 2 . 10 ) 

( 2 . 11 ) 


From these relations, there is obviously a speed increase as the number of legs 
increases from four to eight. However, as previously noted, the speed increase from four 
legs to six legs is threefold, whereas the speed increase from six legs to eight legs is only 
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two-thirds. Herein lies one reason hexapod walking machines are so prevalent over other 
walking machines. 

D. SUMMARY 

This chapter provides background and introductory information helpful for 
understanding the gait planning research undertaken in this study. A general history of 
walking machines is followed v ith a more specijQc survey of hexapod walking machines. 
Terminology and methods typical of the gait planning problem are defined. Other, more 
specific, definitions are presented as material develops throughout the thesis. 

The following chapter contains descriptions of AquaRobot, the model used to 
simulate AquaRobot, and the gait chosen for development in this thesis. 
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m. ROBOT DESCRIPTION AND SIMULATION MODEL 


This chapter is intended as a description of the geometrical features of AquaRobot, a 
description of the AquaRobot model adopted for this study, and a discussion of the 
constraints employed to limit the scope of this study. Additionally, this chapter describes 
the specific gait planning and simulation issues undertaken in this thesis. 

A. AQUAROBOT 

As mentioned previously, AquaRobot is a six-legged, insect-type robot. The body is 
hexagonal and the legs are installed on the sides of the hexagonal frame. Figure 3-1 shows 
the major dimensions and sensor locations of AquaRobot. In this figure, the camera 
manipulator is shown positioned between legs one and two, with leg one pointing directly 
to the reader's right. 

Figure 3-2 is a detailed plan view of AquaRobot. In this figure, the individual leg 
numbers are shown. The legs are placed at 60° increments about the body. This figure also 
includes a detail of the underwater TV camera, ultrasonic ranging device, and lights fitted 
to the manipulator boom mounted on top of the robot's body. The manipulator boom has 
three articulations similar to those found in the legs. The first articulation revolves about a 
vertical axis through the centerline of the robot's body. The next two articulations revolve 
about horizontal axes. Altogether, these three articulations give the camera three degrees 
of freedom. 

Figure 3-3 presents a detailed side view of AquaRobot. Legs one and four are shown 
with dimensions representative of all six legs. The manipulator boom is shown fully 
extended with its maximum reach of 155 centimeters. This figure also shows leg 
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articulations representative of all six legs. The first articulation is located at the 
intersection of the legs and the body. This articulation rotates about a vertical axis and is 
also referred to as articulation one, joint one, the azimuth joint, or the HIP, depending on 
the reference. The typical HIP joint, then, is located 37.5 centimeters outboard from the 
center of the body at 0°, 60“, 120°, 180°, 240°, and 300° increments. The second 
articulation is 20 centimeters outboard of the HEP. It rotates about a horizontal axis and is 
also known as articulation two, joint two, the elevation joint, or KNEEL The third 
articulation, also known as articulation three, joint three, the knee joint, or KNEE2, is 50 
lineal centimeters outboard of KNEEl and also rotates about a horizontal axis. The 
fourth, and final, articulation is known as articulation four or joint four. It is a passive ball- 
and-socket joint that attaches the foot to the leg and is 100 centimeters from KNEE2. 

Figure 3-4 is a detailed view representing a typical leg's structure. In this figure, joint 
rotational configurations are shown. For the HIP, a positive angle represents a clockwise 
(CW) rotation of the joint. For the KNEEl and KNEE2 joints, a positive angle represents 
lifting the leg segment. Although the foot is not actively (motor) controUed, it is allowed 
to rotate passively within ± 45 degrees about joint four. Figure 3-4 also shows cutaway 
views of the previously mentioned harmonic gears, bevel gears, DC motors, and motor 
encoders. 

Table 3-1 provides a synopsis some of AquaRobot's more relevant characteristics. 
AquaRobot, with its present walking and control algorithms, can achieve a maximum 
speed of 7.75 meters/minute when walking on flat surface terrain using a rectangular 
alternating tripod gait. The rectangular terminology refers to the way the foot is moved: 1) 
first the foot is lifted directly upward, 2) then the foot is moved horizontally in the 
direction of travel for one stride, and 3) finally the foot is lowered directly to touch the 
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Figure 3-1. Major Dimensions and Sensors (After [IWA90]). 
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Figure 3-3. Side View of AquaRobot. 











































Figure 3-4. Leg Structure (After [AKI89]). 
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Table 3-1. ylgLM/JoBorP rototype Characteristics (after [iwa901). 


VEHICLE TYPE 

axis-synunetric 

6-legged 

insect-type 

walking 

JOINT TYPE 

4 joints per leg 

joint 1: active, vertical axis, revolute 
joint 2: active, horizontal axis, revolute 
joint 3; active, horizontal axis, revolute 
joint 4: passive, ball-and-socket 

JOINT DRIVE METHOD 

semi-direct drive, DC servo motor 

CONTROL METHOD 

80286-based microcomputer 
controlling hardware via software algorithms 

MAXIMUM WALKING SPEED 

7.75 meters/minute (flat land) 

MAXIMUM ROTATING RATE 

445 degrees/minute (flat land) 

MAIN MATERIAL USED 

anti-corrosive aluminum 

VEHICLE WEIGHT 


SENSORS 

6 tactile sensors 

2 inclinometers 

1 flux gate gyrocompass 

1 pressure sensor 

MANIPULATOR 

CHARACTERISTICS 

TV camera end-effector with 
ultrasonic range finding capability 

MAXIMUM TERRAIN ROUGHNESS 

±35 centimeters 

MINIMUM TERRAIN ROUGHNESS 

± 5 centimeters 

WATERTIGHT DEPTH 

50 meters 

TETHER CHARACTERISTICS 

100 foot length 

42 millimeter diameter 

18 metal wire conductors 
optical fiber link 

1500 kgf tensile strength 

NAVIGATION 

man-machine interface (XYZ inputs) 
dead reckoning 
transponder system 

PURPOSES 

measure flatness of rubble mound 
observe underwater structures 
supervise underwater construction 
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terrain. The foot motion trace, or imaginary path draAvn in the air (or water) by the foot, 
would describe three sides of a rectangle. 

AquaRobot can rotate on its vertical centerline in a CW or counter clockwise (CCW) 
direction if desired. The maximum rotating rate achieved is 445 degrees/minute. 
AquaRobot was designed to walk in terrain with a maximum roughness of ± 35 
centimeters, representative of a typical unleveled rubble mound. A typical leveled rubble 
mound has a maximum roughness of ± 5 centimeters. 

B. SIMULATION MODEL 

The hexapod vehicle simulation model used in this thesis is the prototype AquaRobot 
presently under test at the Japanese PHRI. Figure 3-5 shows plan and side views of the 
simulation model of AquaRobot with the earth- and body-fixed coordinate systems. In this 
study, the earth-fixed coordinate system is known as the world while the 

body-fixed coordinate system is known as the body iXg,yg,Zg). The body coordinate 
system is fixed at the center of the plane defined by the HIPs of all six legs. The x^-axis 
lies along the line joining the HIPs of legs one and four and passing through the center of 
the body. Positive x is in the direction of leg one. The ^^-axis is perpendicular to the 
Xg-axis at the center of the body and passes between legs two and three and legs five and 
six. Positive is in the direction between legs two and three. The z^-axis lies orthogonal 
to the Xg and yg axes with positive z following the right-hand rule convention; positive Zg 
is down. 

Figure 3-6 shows the relationship between the physical and modeled walking 
machine. A representative side view that includes legs one and four is shown. Leg four 
(LEG4) is a scale drawing of an actual AquaRobot leg. Leg one (LEGl) is depicted in 
linkage form. Note from the figure that the joints are numbered fi'om inboard outward. 
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Figure 3-6. Relationship Between the Physical and Modeled Robot. 









with joint zero (JOINTO) located at the robot’s center-of-body. Links are numbered 
following the inboard joint. Therefore, link zero (LINKO) is between JOINTO and joint 
one (JOINTl), link one (LINKl) is between JOINTl and joint two (JOINT2), etc. The 
action of joint four (JOINT4) is not modeled in this study. Instead of modeling JOINT4 
and the entire foot pad, a point foot is used where the junction of link three (LINKS) 
would meet JOINT4. 

At the outset of this study, AquaRobot simulation system parameters were adopted to 
ensure come consistency in program development. The system is described in equations 
3.1 through 3.6. Table 3-2 then lists the variables used in the system description and their 
meaning. The System is defined as a state vector that includes the states of the legs and 

body X and the warning flags F. Then, 

System-{ x,f) (3.1) 

and 

^ = = ( 3 . 2 ) 

The leg states x, are further subdivided into position, velocity, and acceleration vectors 

~ (^*1 >/3,i ,Pa\ > • • • yFk6yFt6 yPa6 )’ 

(3 3 ) 

~ (Ai yPt\ "Pa\ > • yPk6yPt6 »^o6)• 

The body states Xj, are then 

^b={^EyyEy^E^'Py^y¥,^,V,^,P,qj)- (3.4) 

The flag vectors F provide warnings of foot contacts with the terrain and of joint 
limits reached. Therefore, 

P = (Fc^.Fu„) = (F^.Pl) ( 3 . 5 ) 
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The flag vectors are also subdivided into foot contact and joint limit sub-vectors 


Fe =(C,,...,Q), and 


(3.6) 


These system state vectors allow code fi^om several authors to wori; together without 
significant problems. Each author may still refer to parameters using terminology that best 
describes the use of that parameter. For example, this author refers to joint one as the 
HIP, joint two as KNEE 1, and joint three as KNEE3. 


Table 3-2. Simulation System Variable Descriptions. 


3 

Joint Angular Displacement 

k k 

Knee joint (KNEE2), legs 1 through 6 


Elevation joint (KNEEl), legs 1 through 6 



Xp or x„ 

North 

ys or y^r 

East 

2e or 2w 

Down 


RoU 

0 

Elevation 

v|/ 

Azimuth 

1 " 

Body velocity with respect to earth in 

1 ^ 
w 

body coordinates (principal axes). 

p 


r 

Body angular rate in body coordinates. 

c c 

Foot contact flag, legs 1 through 6 

L 

Joint limit flag, all 18 joints 


Many simplifying assumptions were made within this model of AquaRobot. The 
simplifications were made primarily to speed development of the gait planning algorithms 
and graphics routines. However, each program was devised with future expansion in mind. 
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Whenever possible, program code was modularized for future expansion. Specific 
simplifications are addressed in the following paragraphs. 

1. Flat Terrain 

The first simplifications concern the vehicle's operating environment. Although 
the actual AquaRobot was designed to operate in uneven terrain, the simulation model 
initially developed included only flat terrain with no obstacles. The flat terrain is 
graphically displayed using a checkerboard design. The simulation process does allow 
feedback of foot contacts with the flat terrain. Expansion to allow foot contact feedback 
when the foot encounters unstructured terrain is present. 

2. Fixed Body Height 

Because the terrain is flat, the robot's body height and horizontal body angle are 
fixed arbitrarily at the outset of the program. AquaRobot has both selectable body height 
and terrain following capability. Neither of these features is necessary to implement a 
tripod gait. Selectable body height is a feature of the simulation. Terrain following 
capability can easily be added as a code module if future research warrants its use. 

3. Straight Line Path 

Again, for ease in initial program development, a straight line path oriented with 
leg one following the positive x^-axis was adopted. When turning is considered, straight 
line paths between two points is still used. However, the robot is not required to rotate to 
have LEGl align with the desired direction of travel. Some smooth path-following 
techniques were implemented as part of this thesis. 
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4. Constrained Workspace 

Because of its leg design, AquaRobot has the ability to step into or on its own 
feet or legs. Although this feature allows much greater freedom when determining 
footholds, it is not beneficial at the outset of program design. Therefore, the workspace of 
AquaRobot was constrained so no leg or foot overlap was allowed. Other workspace 
constraints, or none at all, are possible by modifying the existing program. 

5. Dynamics 

Finally, the effects of dynamics and hydrodynamics on gait operation are not 
covered. This model encompasses only the kinematic features of AquaRobot. This means 
the model imposes no limits on vehicle acceleration. Similarly, although the manipulator 
boom, tether, lifting lines, and transponder systems were cursorily described, they are not 
modeled nor considered in this thesis. 

C. PROBLEM STATEMENT 

This thesis concentrates on designing an improved alternating tripod gait for 
AquaRobot. Simulation of the improved design is an inherent design goal. Other specific 
design goals include; 

♦ smooth leg motion, 

♦ omnidirectional body movement, 

♦ continuous body motion, and 

♦ limited path following capability. 

The laboratory director from Japan's PHRI agreed that implementing these improvements 
in an alternating tripod gait design would provide immediate increased effectiveness over 
the existing design [TAK93]. 
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The selection of a gait is a very difficult problem. McGhee [MCG68a] showed that 
39,916,800 possible nonsingular periodic hexapod gaits exist. Further, McGhee (MCG85] 
suggests the total number of hexapod gaits is actually a larger, unknown number. This 
thesis, however, is restricted to a single type of gait sequence known as the tripod gait. 
The tripod gait is a singular gait because three legs (hence, the term tripod) are placed 
simultaneously. The gait alternates between sets of three legs. LEGs (1, 3, 5) are 
considered tripod zero (TRIPODO) and LEGs (2, 4, 6) are considered tripod one 
(TRIPOD 1). This zero and one terminology is in keeping with C and C++ programming 
conventions. 

The alternating tripod gait described above is particularly important for walking 
machines, and is another reason for the prevalence of hexapod vehicles [TOD85). Sbc legs 
always allows a supporting tripod, even when half the legs are in the transfer phase. 
Therefore, it allows reasonable walking speed while always maintaining static stability. 

D. SUMMARY 

This chapter outlined the geometrical features, physical constraints, and model 
simplifications of the AquaRohot underwater walking robot. Next, the scope of the work 
this thesis undertakes was described. The following chapter contains the development and 
implementation of the kinematic and kinematics-related concerns of the AquaRobot when 
applied to an alternating tripod gait design. 
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IV. KINEMATICS 


In this chapter, we consider the kinematics problem of computing the position and 
orientation of AquaRoboi's foot, relative to the center of the body, given the joint angles 
for the HEP, KNEEl, and KNEE2. Next, we investigate the more difficult inverse 
kinematics problem; ^ven the desired position and orientation of the foot relative to the 
body center, compute the set of joint angles which will achieve that result. Then a method 
of transforming between the world and body coordinate systems is derived. Next, the 
concept of workspace and its effects on walking is investigated. Kinematics, inverse 
kinematics, coordinate transformation, and workspace discussions are all developed using 
methods from Craig [CRA86]. The chapter ends Avith a discussion of the various body 
postures used in this thesis. 

A. KINEMATICS 

A typical manipulator, in this case a leg for AquaRobot, is actually a set of bodies 
connected in a chain by joints. The bodies are called links. For the purposes of kinematics, 
links are considered only as rigid bodies which define the relationship between two 
neighboring joint axes. Joints, then, form the connection between neighboring links. Joints 
are typically classed as either revolute or prismatic. Revolution about some axis describes 
a revolute joint. Sliding action along some axis describes a prismatic joint. All of 
AquaRobot's\o\TAs are revolute. [CRA86] 

At the end of the chain of links that make up the manipulator is the end-effector. In 
our case, the end-effector is the foot at the end of the leg. Specifically, an end-effector for 
AquaRobot is a point foot at the end of LINK3 on each leg. The position of the foot with 
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respect to the center of the body is described by defining a foot {FOOT} frame relative to 
the center-of-body {CB} frame. A frame is the set of four vectors giving position and 
orientation information. Three of the vectors are typically unit vectors defining the 
principal axes of the fi'ame. The other vector locates the origin of the fiame with respect 
to some other frame, including the origin of the world {W} Srame. [CRA86] 

Four quantities are required to kinematically describe a manipulator. Of these, two 
describe the link and two describe the link's connection between adjoining links. For 
revolute joints, ^ is known as the Join/ variable and the other three quantities are known 
as link parameters. Mechanisms defined with these conventions are typically described 
using Denavit-Hartenberg (DH) notation {DEN55]. There are many related conventions 
that ascribe to the name Denavit-Hartenberg. In this thesis, fi-ame {/} is attached to link i 

and has its origin lying on joint axis i. In other words, link / follows inboard joint i. 
[CRA86] 

Figure 4-1 is a kinematic detail of a typical leg firom the center-of-body to the foot. 
Leg fi-ames are defined according to their respective joints. The (CB) fi-ame corresponds 
to an imaginary fi-ame zero {0}. Frame {0} does not move and is considered the reference 
fi-ame for AquaRobot. Frame {1} is not actually a fi-ame; however, for ease of performing 
the kinematics, each leg is treated as if it were a rotation of 60° about JOESTTO. Therefore, 
LEGl is at 0°, LEG2 is at 60°, LEGS is at 120°, etc. With this in mind. Figure 4-1 shows 
the progression of frames from inboard outward as {CB}, {0}, {HIP}, {KNEEl}, 
{KNEE2}, and {FOOT}. Recall that JOINT4 and the entire foot is not used. 

The leg itself is referenced to two other fi-ames. First is the center-of-gravity {CG} 
frame. This frame locates the center of gravity of AquaRobot, which is not necessarily the 
same point as the center of body. For our purposes, {CG} is set equal to {CB}. Then 
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{CG} is referenced to {W}. The {CG} and {W} frames are shown only for perspective 
and are not used in the derivation of leg kinematics. Transformations between {W} and 
{CB}, however, are examined later in this chapter. 

The general form of the transformation that relates the frames attached to 
neighboring links using the joint variable and link parameters previously defined is 

cos^J -sin^ 0 a._, 

sin 6^ cos cos cos a^_^ - sin a,_, - sin 

sin sin a,_, cos^ sin cosa,_, cosa^.i^/, 

0 0 0 1 




where 

is the angle between Z,_, and Z, measured positive about Z,_,, 
is the distance from Z^_^ to Z, measured along 
d, is the distance from to measured along Z^, and 
^ is the angle between X^_^ and X^ measured about Z^. 

Table 4-1 lists the link parameters for a representative leg. Once the link frames are 
defined and the corresponding link parameters found, values from Table 4-1 are used in 
equation 4.1 to compute the transformations for each link. The {CB} to {0} 
transformation results in 


■Q -5-0 0 O' 

CBj>_ ^0 Co 0 0 

0 0 10 ’ 
0 0 0 1 _ 


(4.2) 


where Q represents cos^o represents sin Oq. Note that the distance a_, between 

(CB) and (0} is zero centimeters because these two frames overlap. The {0} to {1} 
transformation results in 
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(4.3) 





0 

«o' 

7 - 


C. 

0 

0 

0 

0 

1 

0 


0 

0 

0 

1 


where is the distance between {0} and {!}, 37.5 centimeters. 


Table 4-1. Link Parameters yokAquaRobot. 


i 

cc , 

(degrees) 

(cm) 

d, 

(cm) 

0, 

(cm) 

Range 

-1 

^wroRLD 

^WORLD 

dwORLD 

^WORLD 


0 

0 

0 

0 


^0 =0“.60°.120“,180®,240“,300° 

1 

0 


0 

B, 


2 

-90 


0 

B, 

a, =20.0, -106.6®< 6^ < 73.4“ 

3 

0 

^2 

0 

B, 

02=50.0, -156.4“<^3 S23.6° 

4 

0 

^3 

0 

B, 



The other transformations follow the same pattern; 



-$2 0 a, 

0 1 0 
-Q 0 0 
0 0 1 


(4.4) 



-5j 0 flj 

Cj 0 0 

0 1 0 

0 0 1 


(4.5) 
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and 


Q ~S^ 0 a, 

3 5, Q 0 0 

* ~ 0 0 10 

0 0 0 1 


(4.6) 


Because the entire foot is not used, we are not concerned with the action of JOINT4. 
Therefore, setting ^4 = 0 produces a substitution for equation 4.6; 


1 0 0 



0 0 0 



(4.7) 


which provides the necessary transformation from KNEE2 to the FOOT. The final matrix 
used to determine the transformation from (CB) to {4} is found by multiplying each 
of the individual transformation matrices together: 

(4.8) 


The derivation of the final transformation matrix, including intermediate results, is 
shown in equations 4.9 through 4.12. 


(4.9) 




Coi 

Cq, 

0 

^qCq 



’°r= 

•Jo, 

*Soi 

0 

^ 0*^0 


1 u 

1 

0 

0 

0 

0 

> 



0 

0 

0 

1 




■^ 01*^2 

~Soi 

^oCq "^^jCoj 


<5oiQ 

■5o.52 

Co, 

^ 0*^0 ^,^01 

2 '* O '* r 2 * 



-Q 

0 

- 



0 


0 

0 


1 


(4.10) 
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3- 


’r_CB'p O'r X'p I'p _ 

I-*- O'* r 2^ 3^ ~ 


QIQ3 

0)1 *^23 

-0. 

^oO Oi (^1 ^0) 

5o,C23 


Q. 

Ol (^1 ^2^2 ) 

--^23 

-C^ 

0 

-a,5, 

0 

0 

0 

1 


(4.11) 


and 

CBji 

4^ 

Qj"* 

- 0-^ r 

\*p I'p 
a' 4^ 




O1O3 

“Q1O3 

-Oi 



Ol ^23 

_C <’ 
*J0I‘-’23 

Oi 



-‘^23 


0 



0 

0 

0 


^3*^01^23 ^0*^0 *^01 (^1 ^2^2 ) 

"^3*^23 - ^2*^2 
1 


(4.12) 


where 

q =COS0,, 

5, = sin 

q = qc,-5,5-co4^ + dJ, 
and 5„ = 5,q - ^5^ = sin( ^ + 0j). 


Two Matlab programs were written to verify the derivation of the kinematics 
transformations. Program jcoord.m uses equation 4.12 to compute the (x, y, z) coordinates 
of the HEP, KNEEl, KNEE2, and FOOT from joint angles the user inputs. The Matlab 
program kin.m is a function that provides a similar output to the c3.m program when 
called. These two code modules are included in Appendbc B. 


B. INVERSE KINEMATICS 

In the first section of this chapter, we focused on the direct Idnematics of the leg. 
That is, we computed the position and orientation of {4} with respect to {CB}. In this 
section, we focus on the inverse kinematics of the leg. Our goal here is to find the 
required set of joint angles to place the foot, relative to the center of the 

body, when the desired foot position and orientation are known. There are two 
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approaches to solving the inverse kinematics problem. The algebraic approach is basically 
one of manipulating the link parameters into kinematic equations of a form for which a 
solution is known. The geometric approach is to try to decompose the spatial geometry of 
the leg into several lesser plane geometry problems. The geometric approach is the 
method used here. Figure 4-2 shows the plane geometry associated with AquaRohot. 
[CRA86] 

First, consider the triangle formed by the CB, HIP, and FOOT of representative 
LEG2 and shown in the plan view of Figure 4-2. We can apply the Law of Cosines to 
solve for 0^: 

pI +pI =aQ +bf -lajb^ cos(l80°-^j) = a^ +b^ +2aoi, cos^,. (4.13) 


The foot position is given in body-fixed coordinates (p^ ,p ,Pj ). Then, we have 


cos^i =C, = 


^2 r i2 

Px Py ^0 ^1 

2a.h^ 


(4.14) 


where 


*1 = }l(Px -"o cos^o)" +(py-Oo sin Oof 


t-15) 


Finally, we use the two-argument arctangent (Atan2) to find 

=Atan2(±Vl-C^C,). 


(4.16) 


Now we consider the triangle formed by the KNEEl, KNEE2, and FOOT joints as 
shown in the cross-sectional view of Figure 4-2. First, we find expressions for angles P 
and \j/, where 

(4.17) 
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Figure 4-2. Plane Geometry of AquaRobot. 
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Then, 


tan^=|*-, 

and = Atan2(/)^, \), 

where 

b^=\-a,. 

Applying the Law of Cosines again to find \j/; 

= aj + - la^b cos 


Then, we have 


cosy^= 



2aj6j 


and sin y/ = ±-y/1 - cos^ y/, 

where 

b,=ylbl+pl 

Now, substituting equations 4.18,4.22, and 4.23 into equation 4.17 yields 

0^ = 


and 


02 - Atan2(±-y/l-cos^ if/, cos t/] - Atan2(/?,, ). 


(4.18) 

(4.19) 

(4.20) 

(4.21) 

(4.22) 

(4.23) 

(4.24) 

(4.25) 

(4.26) 
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Finally, we again apply the Law of Cosines to the triangle formed by the KNEEl, 
KNEE2, and FOOT joints to solve for 0 ^; 

b] = + a] - la^a^ cos( 180® - ^ 3 ) = + a] + la^a^ cos 0^. (4 27) 


We then have 


cosdj - Cj = 





(4.28) 


and 


^3 = Atm2(±yll-Cl,C,). 


(4.29) 


Because equations 4.16, 4.26, and 4.29 include the possibility of a positive or 
negative result (±), there are eight potential solutions for a single goal. Physical joint 
limitations may prevent using some of the potential solutions. Of the remaining valid 
solutions, the one closest to the present leg configuration is generally chosen. Still, all 
chosen solutions are tested using the direct kinematics routines previously developed. If 
the resulting kinematics solution yields a foot position equal to the desired foot position, 
we accept the inverse kinematics solution as the real solution. We have solved for 0^, 0^, 
and 0j. The solution for 0^ is already known because it is the angle that determines which 
leg is selected. For example, if we know we are trying to place FOOT2, then we know 0^ 
is at 60°. This is because the HIP joints are located at constant, known angles about the 
body. 

C COORDINATE TRANSFORMATIONS 

The AquaRobot simulation uses two coordinate systems in its calculations, the world 
coordinate system and the body (xg,yg,Zg) coordinate system. The world 

coordinate system is used when it is necessary to specify absolute positions or velocities of 
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the body center, feet, or terrain. The world coordinate system is defined with the z^-axis 
positive downward, with the %'axis positive when pointing North, and the y^-axis 
positive when pointing East. 


The body coordinate system, composed of three dimensional local coordinates fixed 
to the robot's body, is used to describe the coordination between the body and the legs and 
to determine stability. The origin of the body coordinate system is defined as the center of 
the plane defined by the HIPs of all sbc legs. The positive x^-axis lies along the line joining 
the CB and the HEP of LEGl. The positive perpendicular to the x^-axis at the 

center of the body and passes between LEG2 and LEG3. The z^-axis lies orthogonal to 
the Xg~ and >> 5 -axes with positive z^ pointing downward. 

The method of transforming between the body coordinate system and the world 
coordinate system is defined through the use of a 4 x 4 homogeneous transformation 
nidcrix, ^ [CRA 86 ]. The matrix may be partitioned into sub matrices 


fyn Wp 


(4.30) 


where is a 3 x 3 rotation matrix and ^Pg is a 3 x 1 position vector [LEE 88 a]. The 
vector '^Pg represents the position of the body in the world coordinate system and Ji? 
gives the rotation of the body relative to the world coordinate system. The p? rotation 
matrix is derived using the X-Y-Z fixed angle orientation convention [CRA 86 ]. Fixed 
angles mean the rotations are specified about a non-moving reference system. For our 
purposes, rotations about the x-axis are known as roll ((}>), rotations about the y-axis are 
known as pitch or elevation ( 0 ), and rotations about the z-axis are known as yaw or 
azimuth (ij/). The rotation matrix then becomes 
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Wn 

B'^xrzUe.r) 


S^gSg+C^C^ — 

—Sg ^$^4 ^(p4 


(4.31) 


Xyf 



Vw 


yB 



^B 

1 


1 


The ^ matrix allows transformation from body coordinates to world coordinates 
using the relationship 


(4.32) 


where the position vectors [x^ l]’^ and [x^ Zg l]^ describe the same 

point in space in world and body coordinates, respectively. The Matlab code module 
b2wtrans.m, found in Appendix B, performs the body-to-world transformation. 

To convert from world coordinates to body coordinates requires a similar 
transformation. In this case, the inverse of the matrix is used. Then, using the form of 
equation 4.32, we find the new transformation process is 


(4.33) 





ys 


yw 

^B 


z^ 

1 


1 


The Matlab code module w2btrans.m, found in Appendix B, performs the world-to-body 
coordinate transformation. 

D. WORKSPACE 

In this section, AquaRobot's workspace is computed. Workspace is that volume of 
space which an end-effector, in our case, a foot, can reach [CRA86]. The workspace is 
also known as the working volume The mechanical limits of the joints restrict leg motion 
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and are a major factor to consider when developing control algorithms for a legged vehicle 
[LEE88a]. Because each leg of AquaRobot has the same geometrical configuration and 
joint limits, the working volumes of the legs are identical. The limits of the joint variables 
for a representative LEG are shown in Table 4-2. 


Table 4-2. Representative LEG Joint Limits. 


LEG Angles (LEGl - LEG6) 

=0°,60°,120°,180°,240°,300° 

HEP Joint Limits 

-60° < e, < 60° 

KNEEl Joint Limits 

-lO6.6°<0j <73.4° 

KNEE2 Joint Limits 

-156.4°< ^3 <23.6° 

FOOT Joint Limits 

- 45 °< 6>4 ^45° 


These joint variable limits, then, separate the reachable area from the unreachable area 
[MCG79]. Reachable areas move with the body. The region included within the reachable 
area is known as the unconstrained working volume (UWV). 

The constrained working volume (CWV) is defined as a subset of the original 
working volume, for each leg, that ensures static stability. Therefore, the CWV sets soft 
limits for each leg so as to exclude points fi'om the working volume that may lead to 
instability [LEE88b]. In our case, the working volume is also constrained to prevent leg 
collisions. An excluded area for AquaRobofs legs, then, is that part of the reachable area 
where, if a foot were placed there, instability or leg collision might result [MCG79]. 

1. Unconstrained Workspace 

The unconstrained horizontal workspace of AquaRobot is shown in Figure 4-3. 
The reachable areas include the sections in the xy plane around the individual HIPs and 
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within the mechanical joint limits. The unconstrained vertical workspace, or Z-plane 
reachable area, depends on the height of the vehicle's center-of-body above the terrain and 
is not shown in Figure 4-3. 

To define the unconstrained vertical workspace, we must define the maximum 
and minimum reach of AquaRobot. If a leg were extended to its fuUest, the added lengths 
of LINKO (37.5 cm), LINKl (20.0 cm), LINK2 (50.0 cm), and LINK3 (100.0 cm) would 
equal 207.5 centimeters. This length does not include the diameter (25.0 cm) or the 
thickness (3.5 cm) of the footpad. However, because of the 45° angle restriction on the 
foot joint, the maximum reach of AquaRobot is as shown in Figure 4-4. The CB, when the 
foot is at its maximum unconstrained reach of 178.2107 centimeters, is a. /0.2107 
centimeters off the terrain. Figure 4-4 includes an illustration of what would be the 
unconstrained vertical workspace if the maximum reach were used. 

If the 45° foot joint angle is used again as the restricting dimension, it is possible 
for the minimum unconstrained reach of AquaRobot to become negative, in the sense that 
the foot can pass completely under the CB, as shown in Figure 4-5. This range of reach 
allows greater flexibility when designing a gait, but significantly increases the complexity 
of the gait algorithms because leg collision avoidance must be considered. Figure 4-5 
includes a drawing of what would be the unconstrained vertical workspace if the minimum 
reach were used. 

Three Matlab programs were written to verify the derivation of the kinematics 
transformations. Program ucwlinkl.m, the results of which are shown in Figure 4-6, 
calculates and plots the unconstrained workspace of LINKl, the HIP to KNEEl link. 
Figure 4-7 shows the results of program ucwlink2.m, which calculates and plots the 
unconstrained workspace of LINK2, the KNEEl to KNEE2 link. Program ucwlink3.m, 
the results of which are shown in Figure 4-8, calculates and plots the unconstrained 
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Figure 4-3. Unconstrained Horizontal Workspace. 
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Figure 4-4. Maximum Reach and Associated Unconstrained Vertical Workspace. 
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Figure 4-5. Minimum Reach and Associated Unconstrained Vertical Workspace. 
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Figure 4-6. LINKl Unconstrained Workspace. 
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Figure 4-7. LINK2 Unconstrained Workspace. 
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workspace of LINK3, the KNEE2 to FOOT link. These three code modules are included 
in Appendix B. 

2. Constrained Workspace 

Now that the UWV is defined, we can define the CWV. The CWV used in this 
thesis results fi'om six basic constraints; 

♦ the CB height is fixed at the point where the maximum reach is achievable, 

♦ the maximum and minimum reach meet the 45° foot joint angle requirement, 

« the terrain is flat, 

♦ the minimum reach is set equal to the radius the HIPs are fi^om the CB, 

« the legs are not allowed to collide or overlap, and 

» the area encompassed by the footpads is included in case feet are added in future 
work. 

Start with the maximum reach obtained when the CB is at 70.2107 centimeters. If the CB 
is then constrained to this value, the minimum reach becomes 36.79 centimeters. For 
simplification, we make the minimum reach equivalent to the radius the HBPs are fi'om the 
CB; 37.5 centimeters. The resulting maximum and minimum constrained reach is shown in 
Figure 4-9 and represents the construed vertical workspace. The height that the leg can 
be raised is obviously restricted when the CB height is fixed. However, this factor is not as 
important, when using flat terrain, as reducing the number of variables when first 
implementing a control program. Changing the CB height to allow traversal of 
unstructured terrain is easily accomplished if required in future research. 

Figure 4-10 shows the constrained horizontal workspace. The constrained 
horizontal workspace is developed using LEGl, LEG2, and LEG6 as an example. Each 
LEG is drawn with its centerline beginning at the CB and bisecting the ± 60° joint limit of 
its HIP. The boundary for the excluded areas is selected as lines that separate the 
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reachable areas for the three legs exactly The actual exclusion areas are then chosen based 
on the edge of the footpad touching these separation lines. This method allows the 
addition of the footpads in the future without changing the workspace algorithms. The 
workspace boundaries then form the sides of a section of an annulus circumscribed about 
the CB. The inner boundary of the annulus is the radius the HIPs make about the CB. The 
outer boundary is the radius of the maximum reach of a LEG about the CB. 

3. Strategy for Constrained Workspace Use 

The primary purpose of the constrained workspace is to define the limits for 
determining the possible length of the stride. If an arbitrary direction is chosen for a 
representative foot to travel, that foot will intersect the edge of the constrained workspace 
at some point. Determining the point of intersection requires algorithms to find the 
intersection of a directed line and a line segment and the intersection of a directed line and 
a section of a circle. Further, if an intersection is found, it must be the intersection at the 
correct orientation of the directed line. Development and implementation of methods to 
generate the required results are abstracted from Kanayama [KAN92]. 

First, we develop the method to determine the intersection of two directed lines. 
In our case, one directed line is derived from the current foot position and desired 
direction of travel. The second directed line is defined as a side of the constrained 
workspace for a given leg, LEGl. Referring to Figure 4-11, we choose an arbitrary 
direction of 0, for the foot to travel within the LEGl workspace. The current position of 
the foot is presumed to have coordinates Pp ={xg,yg), with no Zg component because 
only the horizontal workspace is of interest at this point. The directed line is then 
formed using the expression 

(4 34 ) 
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where x, = Xg and >', = >^ 5 . 

We know tne endpoints of the line segments that form the sides of the 
constrained workspace for LEGl. Using the known endpoints and the two-argument 
arctangent (Atan2) function, it is an easy task to find the angle the line segments make 

with the body-fixed coordinate system. If the angle for one side is taken as 0. , the second 
directed line becomes 

^2 ={(^ 2 .>' 2 ).^ 2 ) (4 35) 

If we let an intersection be {x,y), the distance between the intersection point and either of 
the two directed lines is zero. 

(^->',)cos^,-(x-x,)sin = 0 (4.36) 

(3^~>'2)cos 02-(x-X 2 )sin ^2 = 0 (4.37) 

Combining equations 4.36 and 4.37 0 matrix form; 

sin - cos TX X, sin - >^1 cos 9 ^ 

sin $2 - cos 62 Jlj X 2 sin $2 - cos O 2 

Then, solving equations 4.36 and 4.37 simultaneously yields the intersection point 

(x, sin 9^ -y^ cos ) - cos 0 , 

(Xj sin ^2 ~ >^2 cos ^ 2 ) - cos 02 

(4.39) 

_ - cos 02(^1 sin 0 , -_y, cos 0 )-(-cos 0 ,(x 2 sin 02 -y 2 cos 02 ) 

sin(02 - 0,) 

and 


1 

'"^'~sin(02-0,) 


(4.38) 
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Figure 4-11. Intersection of Directed Line and Line Segment. 
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1 


sinf’, 
sin 02 


(x, sm^i -y^ cos^i) 
(xjSin^j -y^ cos 02 ) 


sin 0, (xj sin - >'2 cos ^ 2 ) ~ sin 02 (x, sin 6 ^ - y^ cos 0^) 

sin(^2 ~ ^ 1 ) 


(4.40) 


The intersection point is then tested to ensure it lies within the desired segment 
of the directed line forming the boundary of the constrained workspace. Kanayama 
proposes that for any three arbitrary distinct points Pi-[^i,yi), nnd 

Pj = (xjj^j) on a directed line, /?, is upstream of P 2 and P 2 is upstream of p^ if and only 
if 

X, cosa+^i sin a < Xj cosa+y 2 sin a < X 3 cosa+yj sin a, (4.41) 

where a is the orientation of the directed line that includes the three points [KAN92]. The 
endpoints for the line segment in question are substituted for points p, and p^ and the 
intersection point is substituted for point p 2 in equation 4.41. Alpha (a) then becomes the 

angle associated with the directed line that makes up the side of the constrained 
workspace. From these values, we determine whether the intersection point is within the 
endpoints of the line segment of interest. The Matlab program segint.m, found in 
Appendix B, performs the segment intersection calculations described above. 

Now we turn our attention to developing the method to determine the 
intersection of a directed line and a circular section. Again, a directed line is derived from 
the current foot position and desired direction of travel. The circular section is defined as 
either the inner or outer radius of the constrained workspace for a given leg, LEGl. In this 
case, we must test for an intersection and then determine whether the intersection is within 
the section of interest. Referring to Figure 4-12, we choose an arbitrary direction of a for 
the foot to travel within the workspace. The coordinates and orientation of the foot are 
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then known; Pp ={(a,i>),a}. The radius and center of the circle of interest are then 
defined, respectively, as r and Pc Front these values, we can determine the 

perpendicular distance d fi-om to the directed line previously described byp,.. using the 
relationship 

d = {yc-b)cosa-{x(;-a)sma. (4.42) 


The radius r and the perpendicular distance d then make up two sides of a right 
triangle with side / unknown. Side / is found using the Pythagorean Theorem; 




(4.43) 


Next, the point p, = where d and / intersect is found using 


X, = x^ +dco; 


{f-)= 


x^ +ifsina 


(4.44) 


3^1 = sini ^ - a I = 3^^ -cos a. 


(4.45) 


The results of equations 4.43, 4.44, and 4.45 are then used to find the intersection point of 
the directed line and the circular section using 

Xi„urcp,^x,+lcosa (4.46) 

>'.m.«,pr = 3'i+^sina (4.47) 

if the foot is located inside the diameter of the circle and 

^in«.«.pr = ^i-^cosa (4.48) 

Jmr«rcp,=>'i-^cosa (4.49) 

if the foot is located outside the diameter of the circle. 
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Because the ends of the circular sections of the constrained workspace are 
known, the intersection point is then tested to ensure it lies within the section of interest 
using the method described in equation 4.41. The Matlab program arcint2.m, found in 
Appendix B, performs the arc intersection calculations just described. 

Once the intersections of the various feet and their respective workspaces are 
known, the maximum stride is determined. The maximum stride is defined as the minimum 
of the maximum strides of the individual feet in a given tripod. Using the maximum stride 
allows all three feet in the tripod to move the same distance in the same amount of time. 
Figure 4-13 illustrates finding the maximum stride possible, given the desired direction of 
travel is -45° and the tripod selected is TRIPODO (LEGs 1, 3, 5). The Matlab program 
maxd25.m, found in Appendix B, determins the maximum stride. 

E. TERRAIN AND POSTURES 
1 . Terrain Considerations 

A walking machine is expected to operate in structured or unstructured terrain. 
The terms structured and unstructured are too diverse to use when describing the possible 
classification and description of terrain. Hirose suggests using five different types of 
terrain classification, as shown in Figure 4-14. First, the O type terrain is a surface which 
supports standard rhythmical walking. The flat terrain model used in this study is of type 
O. Second, the H type terrain includes deep holes in which the legs of the walking machine 
can not reach bottom. A walking machine can not choose the holes as possible footholds, 
but can step over the holes. Next, the P type terrain is described as having poles or rocks 
higher than the walking vehicle itself For P type terrain, the walking machine must avoid 
trying to cross over the poles and rocks. HP terrain is a combination of the H and P terrain 
types. Finally, the G type is considered the usual rough terrain. [HIR 86 a] 
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Figure 4-13. Determination of Maximum Stride. 








Figure 4-14. Terrain Classification (After [HIR86a]). 
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Basically, the terrain models for AquaRobot fall within three general categories: 
1) essentially flat, O type terrain for this study, 2) G type terrain with random irregularities 
of ± 35 centimeters to simulate walking on an unfinished rubble mound, and 3) O type 
terrain with random undulations of ± 5 centimeters to simulate completed rubble mound 
walking. 

2. Operational Postures 

AquaRobot was designed to walk using body postures that optimize the 
particular task or function it is performing. These modes of operation vary according to 
stability, speed, and terrain requirements. There are essentially fi\’e modes of operation 
generally used: 

♦ storage mode, 

« initialization mode, 

♦ walking mode, 

♦ height and width change mode, and 

♦ slim mode. 

AquaRobot requires support when no electricity is supplied to its joints because 
there are no brakes. Hence, AquaRobot rides on a mechanical support platform with its 
legs folded so the minimum volume is used. When AquaRobot has its legs folded on the 
support platform, it is in the storage mode. The storage mode posture is termed RESET in 
this study. When AquaRobot is in the storage mode, the incremental encoders of the DC 
motors are set to an inhial position. The FOOT positions for the RESET posture, plotted 
from the Matlab program reset.m, are as shown in Figure 4-15. [IWA87] 

During the initialization mode of operation, AquaRobot spreads its legs, steps 
off the support platform, and assumes an arbitrary START posture [IWA87]. We define 
the START posture in this study based on the maximum reach and the constrained 
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workspace. Therefore, the START posture allows AquaRobot to achieve the maximum 
possible stride while maintaining an adequate clearance over the flat terrain. The FOOT 
positions for the START posture, plotted from the Matlab program start.m, are as shown 
in Figure 4-16. 

The walking mode is subdivided into two modes. 1) flat terrain walking mode, 
and 2) irregular terrain walking mode. Only the flat terrain walking mode is used in this 
study. The height and width change mode allows changing the stance oi AquaRobot based 
on the type of terrain and the water speed. The tall posture is a narrower stance used for 
rougher terrain. This means the control algorithm can raise the feet higher to step over 
higher obstacles. The short posture is a wider stance used when the current is fast. The 
short posture lowers the vehicle's center of gravity and provides more stability in these 
situations. The tall and short postures are attained simply through adjusting the vehicle's 
center-of-body height over the terrain. [IWA87] 

The slim mode, which is not used in this study, was developed to allow 
AquaRoboH. to pass between closely spaced objects. In the slim mode, the body width is 
decreased to one-half of its original size by rotating some legs so bilateral symmetry is 
achieved. AquaRobot takes on the posture of a crab and uses a slim version of the 
alternating tripod gait to maneuver. [IWA87] 

F. SUMMARY 

The central topic of this chapter was developing and implementing kinematics and 
inverse kinematics for AquaRobot. Next, coordinate transformations required to move 
from body-to-world and from world-to-body coordinate systems were derived. Then, an 
acceptable constrained workspace was designed. Finally, AquaRobot's postures, as applied 
to various terrain types, were discussed. In the following chapter, static stability issues are 
addressed. 
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Figure 4-16. AquaRobot START Posture. 
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V. STABILITY 


A. INTRODUCTION 

One of the main considerations in walking machines is how to achieve stability, that 
is, to keep from falling over [TOD85]. In this study, the static stability concept is 
employed to provide a simple basis for the control of vehicle locomotion. The concept of 
stability mar^ is known as the yardstick for measuring static stability. The value is given 
in the relation between the center of gravity and the polygon of supporting legs, and is 
also governed by the height of the center of gravity and compliance of the legs [HIR86a]. 
For simplicity, however, this study uses the longitudinal stabihty margin 5, as the primary 
approximation of stability. 

In general, it is possible for the center of gravity to go outside the support pattern at 
some instants during locomotion, even if the support joints are constrained to be inside the 
working volumes of the legs [LEE88b]. However, there is some flexibility in changing the 
position and dimensions of the CWV inside the original working volume. This flexibility is 
used to increase vehicle stability [LEE88a]. In our case, the footpad of AquaRobot is 
actually 25 centimeters in diameter, yet we are using a point foot representation. This 
gives us an additional 12.5 centimeters of stability safety margin. As such, mild 
accelerations and decelerations can be accomodated without changing the existing 
algorithms. 

From the previous chapter, we know how to find the maximum stride possible given 
the constraints of the workspace and the mechanical joint limitations. It is possible that the 
maximum stride would allow the CG to move beyond the support pattern. Using the 
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maximum stride alone, then, would result in unstable walking. Therefore, the stride and 
stability calculations are compared for each increment of motion and the lesser of the two 
is chosen as the distance the foot will travel. Although this method greatly reduces the 
amount of motion flexibility, it ensures stability and a reasonably fast gait. 

1. Center-of-Body Versus Center-of-Gravity 

The center of AquaRobot's body is located at the intersection of the plane 
formed by its HEPs and the vertical center of the torso. The CB remains constant during 
any body or leg motion. AquaRobot's center-of-gravity, however, changes with each 
incremental motion of its legs or body. Therefore, at any instant in time, the CG is not 
necessarily located at the CB. 

The PHRI had not completed CG calculations at the time of this thesis writing. 
Because the CG is only needed for stability purposes, the CG is set equal to the CB in tliis 
study. Once the CG calculations are complete, the CG is easily substituted for the CB in 
stability algorithms. 

2. Static Versus Dynamic Stability 

Static and dynamic stability distinguishes the two types of walking machines that 
have been the subjects of recent research. Statically stable systems resist falling down by 
trying never to get in a situation where falling down is possible. Dynamically stable 
systems balance to keep from falling down. Statically stable machines have at least four 
legs and sometimes as many as eight, though more typically six. Dynamically stable 
machines have from one to four legs. The main difference is the use of balance in the 
control of body attitude in dynamically stable machine^. [DON87] 

Of static and dynamic walking, static locomotion is more basic and less 
complicated than dynamic locomotion in terms of control algorithms and motion analysis 
[HIR86a]. It turns out that insects and other many-legged creatures walk with statically 
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stable gaits [DON87]. AquaRobot is an example of an "insect type" machine. Higher 
animals, like horses, cats, and people, all walk using dynamically stable gaits [DON87], 
The AquaRobot simulation presented in this thesis uses a statically stable gait algorithm. 

B. STABILITY MODEL 

The stability model used in this study is similar to that proposed by McGhee 
[MCG86]. The longitudinal stability margin S, is defined as the shortest distance from the 
body's center of gravity to the boundary of the support pattern, measured in the direction 
of travel. From the definition of longitudinal stability margin, a gait is deemed statically 
stable if S,> 0 . Otherwise, it is deemed statically unstable. Vehicle stability, stability 
intercepts, and stability margin are detennined using the known foot positions of the 
supporting tripod and the known body center. All stability calculations use the body-fixed 
coordinate system and substitute the CB for the CG. 

Prior to planning any new leg motion, AquaPobot's stability is evaluated. The 
evaluation consists simply of determining whether the vertical projection of the CB lies 
within an arbitrary support pattern formed from the vertical projection of the three feet in 
a selected tripod. Referring to Figure 5-1, we see the relationship of the supporting 
polygon, in this case, a triangle, to the CB at a particular instant in time for TRIPODO 
(LEGs 1, 3, 5) and TRIPODl (LEGs 2, 4, 6). The CB is represented by the point 
Pi the feet are represented by the points = {x 2 ,y 2 )> P% - 

P 4 =(^ 4 J>' 4 ) Point P 2 is always assigned to either FOOTl or FOOT2, depending on 
whether TRIPODO of TRIPODl, respectively, is selected. Point p^ is always assigned to 
either FOOT3 or FOOT4 and point p^ is always assigned to either FOOTS or FOOT6, 
The CB and feet, then, divide the support polygon into three smaller triangles. 
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Kanayama defines three modes of a triple of points; counterclockwise (CCW), 
clockwise (CW), and colinear [KAN92]. A counterclockwise mode results when the order 
of the points is as shown in Figure 5-2(a). A clockwise mode results when the order of the 
points is as shown in Figure 5-2(b). With these definitions in mind, we can describe the 
area, and thereby, the stability, of a triangle as a triple of distinct points such that 


SiPx.Pt.Pi) 


\ 

2 


1 1 1 

X, Xj 

>'i y2 >-3 


(5.1) 


Expanding equation 5.1 yields: 

^(Pi,P2,Pi) = ^[(^2 -)(>'3 -)- (^3 -)(>'2 ->"1 )]• (5 2) 

The orientation of points is always chosen such that: 

♦ if the CB is inside the supporting polygon, the order is CCW and S is positive, or 

♦ if the CB is outside the supporting polygon, the order is CW and S is negative, or 

♦ if the CB is on the supporting polygon, the order is colinear and S is zero. 

If 5’ is > zero, the tripod is considered stable. Figuie 5-1 illustrates a statically stable 
support pattern. If 5 < zero, the tripod is considered unstable. Figure 5-3 illustrates a 
statically unstable support pattern. The safety margin of 12.5 centimeters discussed earlier 
is not implicitly used in the evaluation of stability. The Matlab function stable.m, found in 
Appendix B, performs the stability test described above. [KAN92] 

Once the stability is ascertained, the stability intercepts and longitudinal stability 
margin 5; are found. The stability intercepts are determined using Kanayama's method for 
finding the intersection of two directed lines, discussed in Chapter IV. The stability margin 
is simply the distance between the CB coordinates and the stability intercepts in the 
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Figure 5-2(b). Clockwise Mode of a Triple of Points (After [KAN92]). 
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Figure 5-3. Unstable TRIPODs. 







direction of travel; 


S, = - CBy)^ - CbJ (5.3) 

The Matlab function stabint.m, found in Appendix B, finds the stability intercepts of a 
given tripod and determines the stability margin. 

C. SUMMARY 

This chapter described the concept of static stability as it pertains to AquaRobot. For 
simplification purposes, the body center was adopted as the vehicle's center of gravity. A 
method for determining static stability, including the longitudinal stability margin, was 
derived. The following chapter describes AquaRobot's current rectangular leg motion 
model and proposes two alternative smooth leg motion models; elliptical and cycloidal. 
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VL SMOOTH FOOT MOTION 


A. INTRODUCTION 

Rough, jagged motion tends to cause abnormal wear of m^hanical mechanisms, 
vibrations resulting from excited mechanical resonances, and inefficiencies of motor 
operation. Therefore, smooth motion is a desirable characteristic. A smooth motion 
function is one which is continuous and which has a continuous first derivative and, 
possibly, a continuous second derivative. Smooth motion for AquaRobot implies smooth 
foot and smooth body motion. This chapter is concerned with developing smooth foot 
motion models. Smooth foot motion, realized through smooth foot trajectories, results in 
decreased wear and tear on motors, gears, and joints and increased motor efficiency. Here, 
trajectories refer to the time history of position and velocity for the three degrees of 
freedom of the feet. [CRA86] 

To guarantee smooth trajectories for the foot, some constraints on the spatial and 
temporal qualities between footholds is required. These constraints are realized through 
the use of curved trajectories. There are many curves with the potential to provide smooth 
leg motion. The criteria for selecting the two curves used here are. 

♦ maintain an orientation normal to the terrmn at liftoff and touchdown, and 

♦ be continuous through the second derivative. 

B. RECTANGULAR FOOT MOTION 

AquaRobot presently uses a rectangular foot motion. Generally speaking, the feet are 
moved upward, forward, downward, and backward along straight lines of travel 
pWASSa]. Figure 6-1 illustrates the resulting rectangular foot motion generated using the 
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Figure 6-1. Rectangular Foot Motion. 










existing algorithm. If (x,^/) is an arbitrary foot point, the new foot point is determined 
using {x+dx,y+dy). The junction of the straight lines forming the rectangle indicate 
points where motor speed control is discontinuous. 


C. SMOOTH FOOT MOTION - ELLIPSE 

An elliptical foot motion was chosen as the primary smooth leg motion model to 
implement. The ellipse meets both criteria established in the introduction to the chapter. 
Figure 6-2 illustrates the construction and result of an elliptical foot motion trajectory. The 
parametric representation of the arbitrary foot position (x,y) begins with 


x = acos0 
y = 6sm0. 


( 6 . 1 ) 


The first derivative of equations 6.1 result in 


— = -asin 

d6 

^=bcosO. 

de 


A small se^ent ds of the ellipse is then calculated using 



and then 



The new foot position is then determined using 

x^^acoie^^e) 

J«w = *sin(^+ ^9). 


( 6 . 2 ) 


(6.3) 


(6.4) 


(6.5) 


78 














The positive trunor axis b of the ellipse is constrained such that the fixed CB height is 
retained and the joint limits are not exceeded. 

D. SMOOTH FOOT MOTION - CYCLOID 


A cycloidal foot motion was chosen as the alternate smooth leg motion model to 
implement. The cycloid meets both criteria established in the introduction to this chapter. 
Figure 6-3 illustrates the construction and result of a cycloidal foot motion trajectory The 
parametric representation of the arbitrary foot position (x,>/) begins with 


x~a{0 - sin 0 ) 
y~b{l-cos^. 


( 6 . 6 ) 


The first derivative of equations 6.6 result in 


= a(l-cos0) 
d0 
dv 

~ a sin 0. 
d6 


A small segment ds of the cycloid is then calculated using 


ds- \{—1 + f —1 


and then 


A0= 


As 




(67) 


(6.8) 


(6.9) 


The new foot position is then determined using 

= a[( 0+ A0) - sin( 0+ A6j)] 
3'«*=a[l-cos(0+A6')]. 


(6.5) 
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The radius a of the circle, of which a point on its circumference traces the cycloid, is 
constrained such that the fixed CB height is retained and the joint limits are not exceeded. 
E. SUMMARY 

In this chapter, two smooth foot motion models were developed: elliptical and 
cycloidal. The next chapter describes the gait planning process for the alternating tripod 
gait used in this study. 
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VBL GATT PLANNING 


A. INTRODUCTION 

"Leg motion planning algorithms are designed to generate the foot trajectory for the 
support and transfer phases and to choose the desired footholds for the transfer legs based 
on the body velocity and the constrained working volume (CWV)" [LEE88a]. Leg motion 
planning is sometimes decomposed into separate gait selection and gait implementation 
tasks. Gait selection is generally regarded as the prior decision to use, or select, a specific 
gait. This study concentrates on the alternating tripod gait because it allows reasonably 
fast walking, yet maintains static stability at all times. Gait implementation, then, is the 
execution of the chosen gait through foothold selection and joint angle determination. 
Because there is so much interaction between the gait selection and gait implementation 
tasks, we choose to combine them into one task termed gait planning. 

B. RECTANGULAR TRIPOD GAIT 

The fundamental walking algorithm for AquaRobot is an up, over, and down motion 
of a group of three legs. As in previous analyses, we call the first group of legs TRIPODO 
(LEGs 1,3,5) with body-fixed coordinates {x3,y1,zi), and (x5,>'5,z5), 

respectively. Similarly, we call the second group of legs TRIPOD 1 (LEGs 2,4,6) with 
body-fixed coordinates (x2,y2,z2), {x4,y4,z4), and (x6,y6,z6). We begin Avith all legs 

touching the ground. A direction for the robot body to travel is selected. Then the distance 
for the robot body to move is determined, possibly based on a desired goal point. From 
this information, the new foot positions are chosen. Then, the body-fixed coordinates of a 
selected group of legs are changed by the difference between the existing foot positions 
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and the new foot positions. The LEGs in a group are then conunanded to move in the 
x-axis direction, (fy in the y-axis direction, and dz in the z-axis direction: 


(xi,^!,^!) = {x\-\-dx,y\-k-dy,z\-^dz). 

(7.1) 

(x3,>'3,z3) = (x3+d&f,_y3+£^,23+tir), and 

(7.2) 

(x5, yS, zS) = {xS+dx,yS+cfy,zS+dz). 

(7.3) 


TRIPODO is generally selected as the first group of legs to move, although this is a purely 
arbitrary decision. Once the TRIPODO feet are free fi'om the ground, the robot body is 
moved along the desired direction of travel. When the TRIPODO feet are firmly planted, 
calculations are begun to move TRIPOD 1, using the commanded motion derived fi-om: 


(x2,y2,z2) = (x2 +dx,y2+ify,z2+dz), 

(7.4) 

(x4,y4,z4}=:(x4+dx,y4+(fy,z4+dz), and 

(7.5) 

(x6,y6,z6) = (x6+dx,y6-i-dy,z6+dz). 

(7.6) 


Alternating the groups of legs, while choosing the same direction, causes AquaRobot to 
travel along a straight line with the body height held constant. [IWA88a] 

We choose to call this gait the Rectangular Alternating Tripod Gait (RATG) 
because the free leg trajectories describe a rectangular pattern. Figure 7-1 illustrates a 
representative leg motion for the RATG. First, the three legs of the free tripod (in this 
case, TRIPODO) are lifted perpendicular to the terrain until KNEE joint limits are reached. 
Essentially, the fi'ee legs are lifted straight upward until the feet are at their maximum 
height. Next, the body is moved in the direction of travel until HIP joint limits are reached. 
Then the free legs are moved in the direction of travel, horizontal with the terrain, until the 
free feet are over the desired touchdown positions. Finally, the fi-ee legs are lowered 






Figure 7-1. Rectangular Tripod Gait Leg Motion. 
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perpendicular to the terrain until contact sensors in each foot indicate that the leg motion 
is completed. 

Gaits can be expressed as functions of distance or time in gait diagrams [TOD86]. 
Figure 7-2 illustrates the distance traveled at ten successive time intervals, /O through /9, 
when a RATG is used. A typical sequence of events for the RATG is: 

♦ body and leg components in START posture, (/O), 

♦ free and lift the legs of TRIPODO (LEGs 1, 3, 5), (/I), 

♦ move the CB in the direction of travel, (t2), 

♦ move the free legs of TRIPODO in the direction of travel, (r3), 

♦ place the legs of TRIPODO, (t4), 

♦ free and lift the legs of TRIPOD 1 (LEGs 2, 4, 6), (/5), 

♦ move the CB in the direction of travel, (/6), 

♦ move the free legs of TRIPOD 1 in the direction of travel, (t7), 

♦ place the legs of TRIPOD 1, (/8), and then the process repeats, (t9). 

In Figure 7-2, a solid dot (•) indicates the foot is touching the terrain and an empty dot (o) 
indicates the foot is free from the terrain. A cross (+) indicates the position of the robot's 
CB. 

Figure 7-3 shows the RATG as a function of time. The motion of each major body 
component (TRIPODO, CB, TRIPODl) is illustrated using a tuning diagram. In Figure 7- 
3, a solid bar (—) along the upper line represents movement of the associated body 
component with respect to the ground. A solid bar along the lower line indicates no 
movement of that particular body component -with respect to the ground. Analysis of 
Figure 7-3 reveals several times when body components are not moving. For example, 
during time neither the CB nor TRIPODl are moving. These areas indicate times when 
joint motors are not powered. Additionally, the CB moves during only one time interval 
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Rectangular Gait as a Function of Distance. 















(t2 or t6) for each TRIPOD cycle. These periods of discontinuous motion are ineflBcient, 
resulting in jerky body movement and slower over-the-ground speed. 

C. DISCRETE TRIPOD GAIT 

The Discrete Alternating Tripod Gait (DATG) was developed as the natural next 
step after the rectangular gait. For our purposes, non-continuous motion of the CB is 
known as discrete motion. The DATG provides for discrete motion of AquaRobot's CB. 
The distance the CB travels is constrained by workspace and stability limits. For ease of 
calculation, the three feet in a tripod describe a regular triangle. This approach does not 
necessarily lead to the maximum possible stride or the minimum time to cover a distance. 
Nevertheless, stability is easily assured. The DATG includes an elliptical foot motion 
trajectory as a means of alloA^g continuous foot motion between ground contact points. 
A typical sequence of events for the DATG is: 

♦ body and leg components in START posture, (tO), 

♦ plan an elliptical path for and move TRIPODO to its destination, (tl), 

♦ move the CB in the direction of travel until a limit is reached, (t2), 

♦ plan an elliptical path for and move TRIPOD 1 to its destination, (G), 

♦ move the CB in the direction of travel until a limit is reached, (t4), and 

♦ continue repeating the process, (t5 through t9). 

Figure 7-4 shows the DATG as a function of distance. Figure 7-5 shows the DATG 
as a function of time. The major difference between the rectangular and discrete gaits is 
the leg motion. Except for periods when the feet are leaving or contacting the terrain, or 
when only the CB is moving, the feet are in continuous motion. The leg joint motors for 
the free legs in the DATG, then, are operating almost continuously. However, the CB 
motion is discontinuous in both gaits. 
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D. CONTINUOUS TRIPOD GAIT 


The Continuous Alternating Tripod Gait (CATG) was designed to allow continuous 
motion of the CB, independent of the grouped leg (tripod) motions. The sequence of 
events is similar to the DATG, except the CB moves continuously. TRIPODs zero (0) and 
one (1) still alternate and the foot trajectories are still elliptical; however, the CB is not 
allowed to halt once the walking begins (unless the goal point is reached). The supporting 
tripod provides the motion for the CB. 

The CB motion is synchronized with the motion of the tripods. The desired foot 
positions are used to generate an elliptical trajectory for the selected free tripod in the 
transfer phase. Then the distance between the existing and desired foot positions is divided 
into small segments. The length of these segments is then used to determine the motion 
the CB makes in the direction of travel in the xy plane. The body height is held constant, 
so the CB is not moved in the z plane. For every increment of free tripod foot motion in 
the xy plane, the supponing tripod increments the CB motion so the CB completes one- 
half the distance the free tripod travels in the same time period. The CB moves one-half 
the foot distance because of workspace restrictions. Actually, the supporting tripod joints 
are moving, thereby moving the CB indirectly. The increments are chosen arbitrarily small, 
in our case 100, so the CB appears to move continuously. The CB moves in the direction 
of travel during each tripod supporting phase and when all six legs are on the ground. 

From observation of the CATG, the maximum possible stride is 146.04 centimeters. 
Therefore, the maximum possible CB motion is 73.02 centimeters. The AquaRobot 
simulation can be run with a default maximum stride value slightly less than 146 
centimeters to compensate for the edges of the footpads touching at the extreme limits of 
the workspace. 




A typical sequence of events for the CATG follows. No time steps are given because 
all major body components are continuously in motion. Initially, the body and leg 
components are in the START posture. The desired foot positions for each foot in one 
tripod are determined using workspace and stability constraints. The stride chosen is the 
minimum distance any one of the three feet in a tripod grouping can move in its individual 
workspace. TRIPODO is arbitrarily selected and an elliptical path is planned to move its 
feet one increment towards their new destination. For the first step, the CB is moved a 
distance equal to the maximum stride. Therefore, then new location for the CB to travel to 
is calculated using 

CB(x^) = CB(x^) + {stride) *cos{direction), and (7.7) 

CB(y^) = CB{y^) + [stride) * %\n{direction). (7.8) 

TRIPOD 1 then moves the CB the distance required to attain the new CB 
coordinates. For the next steps, the CB is moved a distance equal to one-half the 
maximum stride. In this case, the new location for the CB to travel to is calculated using 

CB(x„) = CB(x„) + |^^^^j*cos(fi(/rec/7(?n), and (7.9) 

CB(y^) = C5Ck,,) sin(r/;recnow). (7.10) 

After each incrementation of the CB position, its location is compared to the goal point. If 
the distance from the CB to the goal point is less than would result if the maximum stride 
were used, the lessor distance is used. Then, the new stride length becomes twice the 
d' stance from the CB to the goal point. 

Figure 7-6 shows the CATG as a function of distance. The motion of the legs is the 
same as the DATG. Figure 7-7 shows the CATG as a function of time. This figure 
illustrates the continuous motion of the major body components. 
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E. SUMMARY 


In this chapter, the gait planning algorithms for the rectangular, discrete, and 
continuous alternating tripod gaits were described. The next chapter implements the 
discrete and continuous gait planning algorithms in computer code. 
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Vm. AQUAROBOT SIMULATOR 


A. INTRODUCTION 

In this chapter, the AquaRobot Simulator program is presented. Initially, the 
simulation facilities, tools, and methods are described. Then the internal structure and flow 
of the program is discussed. One part of the program is a three-dimensional (3D) stick- 
figure simulation of the DATG. Another part of the program is a 3D stick-figure 
simulation of the CATG. A complete listing of program code is found in Appendix C. A 
separable User's Guide, with instructions on how to use the program, is included as 
Appendix D. 

B. SIMULATION FACBLITIES 

A critical part of this thesis included integrating the gait planning module design with 
a previously developed graphics simulation program [DAV93]. As a result, some graphics 
code required revision or augmentation. Additionally, the gait planning code modules 
required some transportability between the AquaRobot control computer (NEC 486-based 
Personal Computer with MS-DOS operating system) and the AquaRobot Simulator 
(Silicon Graphics Personal Iris Workstation). To ensure some level of transportability 
between the personal computer and the workstation, the ANSI standard C format was 
adopted. The graphics code modules are specific to the Silicon Graphics Workstation. 

Many portions of the gait planning algorithms were originally written and tested in 
Matlab. a "C"-based development language. Appendk B contains the Matlab code. Once 
the basic algorithms were verified, they were converted to the language for 
implementation in the AquaRobot Simulator. The C"^ language was used for grapWcs, for 
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interfacing between the gait planning and graphics modules, and whenever specific objects 
(legs, body, etc.) could be defined. Appendix C contains C** code. 


C MODULE STRUCTURE 

Figure 8-1 contains a flow chart of the AquaRobot Simulator program. The 
simulation is essentially one program with options for either the DATG or the CATG. 
Typing aqua starts the simulation. The user then determines the distance, and thereby the 
direction, the simulated AquaRobot travels at the outset of the program; the goal point, in 
(x,^) coordinates, is entered from the keyboard. Any arbitrary distance (direction) is 

acceptable. Once the graphics portion of the program begins, the user can select the 
camera viewing angle. When the simulated AquaRobot reaches the desired goal point, it 
stops. A description of each functional module follows. 

The simulator files are separated into four modules. The files that comprise the 
Makefile, Graphics, and Matrix Manipulation Modules are only cursorily addressed here. 
The Gait Planning Module is described in more detail. 

1. Makefile 

The Makefile allows the make utility to intelligently compile the program. This 
file includes instructions for how and when to compile the various files that comprise the 
AquaRobot Simulator. The AquaRobot simulation program uses the UNIX make utility to 
link together the 27 individual files with the graphics, math, and standard input-output 
libraries. In this case, the Makefile generates the executable program aqua. 

1. Graphics 

The 3D stick-figure graphics code was originally written by Sandra Davidson 
[DAV93]. The graphics files (code modules) are included in Appendix C for easier 
reference. Some file names were changed to avoid software configuration management 
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Figure 8-1. AquaRobot Simulator Flow Chart. 
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problems. Additionally, some graphics code was modified because of the requirements of 
the gait planning code. For example, the FARCLIPPING value and the CAMERA viewing 
angles were changed in the bot.H file and the CB height was changed in the AbBody.C 
file. Because the Graphics Module polls the Gait Planning Module, a suitable interface 
between the two was constructed in the Kinematics.H and Kinematics.C files The 
graphics code consists of the following files; 

♦ AbBody.H and AbBody. C, 

♦ AbLeg.H and AbLeg. C, 

♦ AbRigidH and AbRigid C; 

♦ bot.H zndbotC, 

♦ Kinematics-H and Kinematics. C, 

♦ LinkH and Link. C; 

« LinkO.H and LinkO. C; 

♦ Linkl.H and Linkl. C; 

♦ Linkl.H and Linkl.C\ and 

♦ Links.H and Links. C. 

3. Matrix Manipulation 

The matrix manipulation code was also originally written by Sandra Davidson 
[DAV93]. It provides 4x4 matrix multiplication capability. This code is an essential part 
of the graphics code, although it is not used in the gait planning code. The Matrix 
Manipulation Module includes the following files, found in Appendix C . 

♦ MatrixMy.H and MatrixMy. C. 
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4. Gait Planning 

The Gait Planning Module (GPM) consists of the following files: 

♦ arconst.H; 

♦ cajunc.H and arfunc. C; and 

♦ artpgait.H and artpgait. C. 

To support easier reference and nraintenance, most constants are grouped together into 
the file arconst.H (aquarobot constants). 

The arfunc.H/C (aquarobot functions) files contain 13 functions. The min and 
max functions simply return the minimum or maximum of two expressions, respectively. 
The ellipse function calculates the incremental foot trajectory along an elliptical path 
between the current foot position and the desired foot position. The kinematics function 
performs kinematics for the Gait Planning Module. Kinematics for the Graphics Module 
are performed in the Graphics Kinematics.H/C files. The inv kinematics fimction 
performs the inverse kinematics operations. The world botfy function provides coordinate 
transformation fi-om the body-fixed coordinate system to the world coordinate system. 
The bodyjworld function provides coordinate transformation from the world coordinate 
system to the body-fixed coordinate system. 

The maxdistance_25cm function determines the maximum stride for the 
workspace defined by the 25 centimeter footpad. The maxdistance_45cm function 
determines the maximum stride for the workspace defined by the 45 centimeter footpad. 
The arcint function calculates the intersection of a directed line and an arc segment. The 
segint function calculates the intersection of a directed line and a line segment. The stable 
function calculates the stability of a tripod. The staparam function determines the 
longitudinal stability margin and the x mdy intercepts of the directional ray originating at 
the CB and terminating at the point of intersection on the selected tripod. 
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The artpgait.H/C (aquarobot tripod gait) files 15 functions. The get^oal 
function allows the user to input the desired goal point to which the AquaRobot simulation 
walks. The getjnerm function allows the user to choose between the DATG or the 
CATG. The getJdotchoice function allows the user to determine whether the 25 
centimeter or 45 centimeter footpad is used. The gait algorithm function executes either 
the DATG or the CATG gait algorithms, as previously determined by the user. The 
robot model function initializes the joint angles for the simulation. 

The disc setjlag function initializes the flags used during the DATG algorithm. 
The disc_gait_planning function determines the walking parameters, such as stride length 
and direction, used during the DATG algorithm. The disc tripod motion function 
calculates and executes incremental joint motions necessary to move the AquaRobot 
simulation using the DATG. 

The cant set Jlag function ixutializes the flags used during the CATG algorithm. 
The contjgait j}lanning function determines the walking parameters, such as stride length 
and direction, used during the CATG algorithm. The cont tripod motion function 
calculates and executes the incremental joint motions necessary to move the AquaRobot 
simulation using the CATG. 

Some functions in the artpgait.C file are used only for printing out the status of 
joints, feet, etc. These functions include print status, printjwalkpara, print_grm_data, 
print Joint data. 

D. SUMMARY 

This chapter described the AquaRobot simulation program. A discussion of the 
simulation facilities, tools, and methods followed. Then, the organization of the program 
was described. The final chapter contains results and concluding remarks. 
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K. CONCLUSIONS 


A. RESULTS 

Both the DATG and CATG algorithms work successfully. Most of the coding is 
shared between the two algorithms. The Gait Planning Module, of which the DATG and 
CATG are a part, requires approximately 1600 total lines of code. Graphics code is 
excluded from the line count. 

Figures 9-1(a), 9-1(b), and 9-1(c) juxtapose the diagrams for the rectangular, 
discrete, and continuous alternating tripod gaits. It is obvious from the figures that there is 
a progression of improvement in continuous CB motion fro the RATG to the CATG. 

Many areas of the RATG and DATG gait diagrams show no major body component 
(TRIPODO, CB, TRIPOD 1) motion. These areas indicate times when joint motors are 
operating discontinuously, i.e., motors are stopped. Starting and stopping the joint motors 
results in rough, jagged motion. This, in turn, tends to cause abnormal wear of mechanical 
mechanisms, vibrations resulting from excited mechanical resonances, and inefGciencies of 
motor operation. Smoother, more continuous motion results in decreased wear and tear 
on motors, gears, and joints and increased motor efficiency. The CATG, with elliptical 
foot trajectory, allows these benefits to occur. 

Figures 9-2 through 9-7 present the motion of the individual leg joints (HIP, KNEEl, 
and KNEE2) for the first few tripod cycles of the DATG and the CATG when walking 
with LEGl along the x-axis. The upper plot in each figure (a) shows the DATG; the lower 
plot in each figure (b) shows the CATG. As seen in Figures 9-2 and 9-5, there is no HIP 
motion for the legs of the tripod that lie on the x-axis: LEGl in TRIPODO and LEG4 in 
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to tl t2 t3 t4 t5 t6 t7 t8 t9 

Figure 9-l(a). Rectangular Alternating Tripod Gait. 



to tl t2 t3 t4 t5 t6 t7 t8 t9 

Figure 9-l(b). Discrete Alternating Tripod Gait. 


to tl t2 t3 t4 t5 t6 XI t8 t9 


Figure 9-l(c). Continuous Alternating Tripod Gait. 







Figure 9-2(a). LEGl Joint Angle Motion for DATG. 
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Figure 9-7(a). LEG6 Joint Angle Motion for DATG 
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Figure 9-7(b). LEG6 Joint Angle Motion for CATG, 









TRIPOD 1. A comparison of joint angle motion for the two gait algorithms indicates; 

♦ there are periods of no joint motion in the discrete gait, 

♦ there are no such periods in the continuous gait, and 

♦ in both gaits, there are discontinuities when the feet touch the ground. 

The goals of this thesis, outlined in Chapter in, were to provide simulation of an 
improved alternating tripod gait that included; 

♦ smooth leg motion, 

« omnidirectional body movement, 

♦ continuous body motion, and 

♦ limited path following capability. 

These goals have been met en route to the completion of this thesis. 

B. RESEARCH CONTRIBUTIONS 

This thesis is a direct outgrowth of a cooperative research effort between the Naval 
Postgraduate School and the Japanese Port and Harbor Research Institute to enhance the 
hardware and software capabilities of the AquaRobot underwater walking machine. 

The PHRI originally developed AquaRobot to replace hard-hat divers constructing 
tsunami barriers in the hazardous underwater environment off Japan's coast. Although 
AquaRobot has demonstrated significant ability as the first hexapod underwater walking 
machine, it is still unable to perform its designed task more efficiently or less costly than 
human divers. AquaRobot speed and agility enhancements derived from this thesis will 
result in a more eflScient and less costly machine. This translates directly into reduced risk 
of human injury and decreased construction costs. 

Although walking machine research and construction is extensive, AquaRobot is the 
first walking robot with a practical use. As such, improvements to AquaRobot directly 







result in concept fulfillment and favorably demonstrate the usefulness of robots, and 
walking robots in particular. 

This opportunity to work and study with the PHRI lays the foundation for future 
cooperative U.S.-Japanese research activities in robotics. Even marginal headway towards 
improving U.S.-Japanese electronics and robotics research efforts will represent a 
significant cooperation milestone reached. 

C. RESEARCH EXTENSIONS 
1. Required Future Work 

Towards the end of the thesis research, an opportunity to visit the PHRI in 
Japan presented itself [PHR93]. During the week-long visit, three sigiuficant dimensional 
changes to the AquaRobot were discovered that affect the workspace, stability, and 
maximum stride results in this thesis. The dimensional changes to AquaRobot were the 
result of ongoing research at the PHRI into the machine's mechanical attributes. None of 
the changes impact the derivation of the kinematics, inverse kinematics, stability, stride, or 
gait algorithms. However, the numerical values used in the calculations are slightly 
different from those culled fi'om papers and blueprints. The three changes were: 

♦ the footpad diameter increased from 25 centimeters to 45 centimeters, 

♦ the footpad angle decreased fi’om ± 45® to ± 22.5°, and 

♦ the lengths of LINK2 and LINK3 increased 1.8 and 1.0 centimeters, respectively. 

The LINK dimension changes and the footpad angle change are reflected in 
Table 9-1. LINKO and LINKl were not changed. Figure 9-8 shows the reduced 
workspace resulting fi'om the dimensional changes. The original workspace for LEGl is 
shown with dashed lines to emphasize the change in working area. 
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The dimensional changes affect the workspace, stability, and maximum stride. 
First, the new workspace is reduced from its previous size. However, the workspace is 
used in the same manner as before. Next, stability is increased because the radius of the 
foot pad almost doubled. Because stability is measured at the center of the footpad, the 


additional radius actually imparts a greater stability safety margin. Finally, because of the 
footpad angle change, the maximum stride length is reduced from 178.21 centimeters to 
147.57 centimeters. The total workspace area is reduced from 10367 to 5509 square 
centimeters, or approximately one-half its previous size. 


Table 9-i. New Link Parameters yorAquaRobot. 


■ 


WL 

Wk 

Wk 

Range | 

-1 

^WORLD 

^(VORLD 

^WORW 

^WORID 


0 

0 

0 

0 

wm 

^0 =0^60^120^180®,240®,300° 

1 

0 


0 

0, 

Oo = 37.5, -60°< e, < 60° 

2 

-90 


0 

e. 

a, =20.0, -105.6°<^2 <74.4° 

3 

0 


0 

9 , 


1 4 

0 

^3 

0 

9 . 



a. Unconstrained Workspace 

This thesis was developed using a constrained workspace. The workspace 
must eventually be unconstrained to determine which gaits, without sacrificing vehicle 
stability and energy efficiency and while maintaining continuous body motion, result in the 
fastest speeds for a hexapod underwater walking robot. This future work should primarily 
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Figure 9-8. New Constrained Horizontal Workspace. 









be directed towards investigating whether there is an analytical method for optimizing the 
leg cycling of a walking vehicle for an arbitrary given maneuvering specification while 
moving over rough terrain. Successful realization of this research would result in on-line 
optimization of foot placement. 

b. Six DOF Body Movement 

Statically stable alternating tripod gaits were planned and implemented in this thesis. To 
ease imtiai gait implementation, flat terrain, fixed body height, fixed body orientation, and 
straight line path constraints were placed on the vehicle. Constraints must gradually be 
removed to allow rough-terrain walking capability with an arbitrary orientation. Future 
work should include lifting the constraints on body movement: allow the body orientation 
and body height to change. If these efforts are completed, the AquaRobot simulation can 
then include navigation over irregular terrain. 

c. Dynamic Effects 

A study of the effects of the aqueous medium, ocean currents, tether, and 
camera boom loading on AquaRobot is advised. The greatest limiting effect on 
AquaRobot's speed may be the effects of the aqueous medium, including the effects of 
underwater currents, on the movement of the body and the legs. Environmental effects 
include the salt water medium itself and the approximately two knot subsurface ocean 
current typical off Japan's coast. Non-ideal characteristics include the effects of the tether 
and the main body camera boom on efficient walking. 

2. Potential Future Work 

The following paragraphs describe potential future work. The work is potential 
because research has shown that alternating tripod gaits are a simple way to achieve 
reasonable walking speed while maintaining static stability. 







a. Other Stable Gaits 

This thesis research concentrated on statically stable alternating tripod 
gaits. Additionally, only equilateral triangles (tripods) were considered. Considering 
isosceles, or other irregularly shaped, triangles may result in a more eflScient use of stride 
and stability parameters towards increasing vehicle speed. Furthermore, because 
AquaRobot's six legs are located at 60 degree intervals, at least two bilateral symmetries 
are attainable. Symmetry is attained when a fore and aft pair of legs is defined or when any 
three legs are distinguished from the other three legs. One potentially important question is 
whether these alternative symmetries can be exploited to increase overall speed when 
arbitrary vehicle motion is desired. 

b. Unstable (Free) Gaits 

A potential future research topic includes determining whether there are 
optimality improvements when gaits are selected that result in momentary vehicle 
instability. The important question here is whether the viscous effects of the aqueous 
medium (especially the natural buoyancy of salt water) allows gaits which would be 
unstable in air to be used in the water. 
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APPENDIX A: 


AQUAROBOT OVERVIEW AND fflSTORY 


AquaRobot was conceived and designed at the Japanese Port and Harbor Research 
Institute (PHRI) in 1984. Its main purpose is to replace divers in carrying out underwater 
inspection work associated with various port construction projects. Specifically, the 
AquaRobot project is an outgrowth of a multi-billion dollar project to construct a tsunami 
barrier at the entrance to Japan's Kamaishi Bay. The primary design function of 
AquaRobot is to measure the flatness of underwater rubble mounds used as foundations to 
support concrete caissons that form the tsunami barrier. A secondary design function is to 
provide for observation of the underwat ;r construction effort using a television camera. 

Figure A-1 shows a representation of the expected underwater operation of 
AquaRobot. Navigational accuracy of AquaRobot is attained using a transponder system 
that allows position errors of less than ten centimeters at a distance of 300 meters 
[AKI89]. Every two steps, AquaRobot receives an updated navigational position from the 
transponder system [IWA90]. AquaRobot is tethered to a control platform, in this case a 
ship, vwth a 100 meter cable that includes optical fiber and metal wire links [IWA88b]. 

Three models of AquaRobot have been constructed to date. The first model, 
completed in 1985, was an experimental version developed for concept exploration and 
validation. It is used solely for overground testing. The experimental model weighs only 
280 kilograms, yet can cany one person on its back. The second model, constructed in 
1987, is the prototype version. The prototype is almost twice as large as the experimental 
model, mainly because of the increased leg length and the addition of the camera boom 
assembly, tether linkage, and lifting apparatus. The prototype is watertight to 50 meters 
depth, thereby causing additional bulk. The prototype weighs 700 kilograms. The third 
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Figure A-1. Underwater Operation of AquaRohot (After [IWA88a]). 
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model, finished in 1989, is a light weight version of the prototype. Its improvements over 
the prototype include approximately half the weight and cable runs made inside leg 
articulations. [IWA90] 

The body of AquaRobot is constructed of anti-corrosive alununum and is hexagonal 
in shape. Sk legs are attached at 60 degree increments about the body. Legs are numbered 
beginning with leg one and proceed in a clockwise direction through leg sk. Each leg has 
three degrees of fi’eedom and is constructed in a fashion similar to manufacturing 
manipulators. The three degrees of freedom are attained with three articulations, or joints 
per leg. The first joint rotates about a vertical axis and the next two joints rotate about 
horizontal axes. The joints are semi-direct drive with DC motor actuators and combination 
harmonic and bevel gears. Each of the 18 joint motors requires 70 Volt DC power, 
provided from the control platform via the tether cable. Each motor includes an encoder 
that produces 100 pulses per revolution. The motor control system is shown in Figure 
A-2. A motor driver for each motor counts the pulses from the controller, an 8086-based 
microcomputer, and from the motor encoder and drives the motor until the differential 
pulse count is zero. Using this method, the motors, and thereby, the joints, are rotated to 
the position the computer commands. [IWA88a] 



Figure A-2. Motor Control System (After [AKI89]). 
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Each of the six legs has an end-efiector consisting of a 25 centimeter diameter, three 
centimeter thick, disk that acts like a foot. This foot operates from a passive ball-and- 
socket joint at the end of the leg. This passive joint constrains the motion between the 
long axis of the leg and the surface of the terrain to plus or minus 45 degrees. 

An additional three-degree-of-freedom manipulator is attached to the top of the body 
and provides a platform for a TV camera and ultrasonic ranging device. Three lilting eyes 
are mounted equidistantly around the upper body and a tether interface box completes the 
top portion of the body. 

AquaRobot uses four sensor types. A touch, or contact, sensor is installed near the 
junction of each leg and its foot. The contact sensors have a two millimeter plunger, but 
require only 0.5 millimeters of travel for activation [TAK93]. Two inclination sensors, or 
inclinometers, are installed in the body to determine the angle of inclination with respect to 
the terrain. A flux gate compass is also installed in the body to sense direction. Finally, a 
pressure sensor is included in the base of the body to accurately measure the water depth. 
[IWA88a] 

AquaRobot's current control program is as shoAvn in Figure A-3. The program 
includes walking and operating algorithms compiled and assembled in the BASIC 
programming language. There are essentially three sections of this program. First, there is 
the Walking Algorithm Program. The main purpose of this section of the program is to 
receive inputs from a human operator and convert those inputs into coordinate values for 
the robot's feet. Then the Robot Language section of the program is called. The Robot 
Language section includes fundamental command functions that perform operations such 
as changing the speed of motion of the leg, rotating the leg joints through a given angle, 
and determining whether a touch sensor has contacted the ground. The Robot Operating 
System Program then performs the necessary operations to actually move the robot. 








Calculation of joint angles, conversion of angles to motor pulses, and providing the pulsed 
output to the motors is included. Finally, the Robot Operating System Program contains 
limited graphical display and simulation functions. [AKI89] 


WALKING ALGORITHM PROGRAM ROBOT OPERATING SYSTEM PROGRAM 



Figure A-3. AquaRobot Computer Control Program (After [AKI89]). 

The prototype AquaRobot was actually tested in the field environment in February 
1990. The test area chosen was the temporary storage area for completed caissons at the 
Izumi working area in the Kamaishi Port of the Iwate Prefecture. There, the sea bottom 
was covered with a rubble mound leveled with the same leveling machine used on the 
actual barrier site. The average depth was 24 meters. The navigational test is performed by 
having AquaRobot walk fi'om one point to another along a previously measured path. 
Rectangularly shaped one-centimeter by two-centimeter steel plates are used to mark the 
waypoints along the path. Table A-1 shows the results of navigation and walking speed 
accuracy during the field test. Table A-1 shows a maximum error of +21 centimeters. This 
error includes the average navigational error of the transponder system, ±10 centimeters. 
Errors may derive fi-om aqueous influences (such as ocean currents), tether influences, or 
cumulative foot positioning offsets. From this table, it is obvious that walking speed 
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depends upon step height and step length. The maximum walking speed attained was 1.43 
meters per minute. [IWA90] 


Table A-1. Navigation and Walking Speed Accuracy (After [iwa901). 


Step Length 

(cm) 

Step Height 

(cm) 


Measured 

Destinadon 

(cm) 

Error 

(cm) 


15 

35 

X34821 

X34816 

-5 

0.61 

Y-7251 

Y-7264 

-13 

20 

35 

X 35029 

X35044 

+15 

0.67 

Y-8232 

Y-8218 

+14 

15 

35 

X34821 

X34822 

+1 

0.67 

Y-7251 

Y-7249 

+2 

20 

35 

X35029 

X35037 

+8 

0.76 

Y-8232 

Y-8231 

+1 

20 

25 

X32810 

X32811 

+1 

1.27 

Y-7371 

Y-7350 

+21 

20 

25 

X33000 

X32981 

-19 

1.43 

Y-7500 

Y-7504 

-4 


Figure A-4 shows the results of AquaRobot walking a planned path. The planned 
path included start and goal points and six waypoints. The actual walking distance was 9S 
meters. In this case, the average walking speed was 1.32 meters per minute. [IWA90] 
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Figure A-4. Field Test of Planned and Measured Path (After (IWA90]). 
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APPENDIX B: 


MATLAB PROGRAM LISTINGS 


This appendix contains the high level source code used in development and testing of 
the AquaRobot gait planning algorithms. The source code is in Matlab. a ''C"-based 
development language. The Matlab code is not intended for subsequent use in either 
AquaRobot or the AquaRobot Simulator. 

Matlab is a registered trademark of The Math Works. Incorporated . It does not produce 
stand-alone executable code, but is instead an interpretive program designed for numerical 
problem solving and is especially adept at matrix manipulation. The source code in this 
appendix requires Matlab for execution. 
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S Title: irciittjn 
S Inputs: (1^,4.5.6.7) 

SOutpuU: [U^] 

SPuipose: ThisMatUbfiinctioodetenninestheinleneGtionofsny 
% and an are segnieoL 

% 

fimclioa (intcpt, dist, flag] > aretDi(foot, dine, oenbod, rad, tinneg, limpoa, flnum) 

% 

% intcpt = (x,y) oootdinates td'intereept 

% dist = distance between foot and intercepts 

%flag~ 1 if intercept found; flag >*0 if no interoept found 

% foot ~ (x,y) foot compo n ents 

S direc - direction of interest 

Scenbod’* (x,y) coordinates c^ccater of body 

S rad ° radius oi ciicle 

% limneg/limpoa ~ endponUs of are sepnent 

% flnum = foot 1 tfarou^ 6 

% 

limit ^ 213 ; H maximum possible distance 
flag = 0; 

diiec = direc*pi/180; 

% 

% Determine if the foot foils on the are segment 
footrad = sriit(foot(l,l)^ + foot(l,2)^); 
if (rad fboUad) 
flag - 1; 

intcpt - [foot(l,l), foot(l,2)]; 
dist = limit; 
end 
% 

if (flag — 0) 

perpd - ((cenbod(l,2) • foot(l,2))*cas(direc)) • ((oenbod(l,l) • fool(l,l))*sin(direc)); 
ifperpd<-rad 

len^ ” s<pt(rad^ - perpd^); 

% 

% find (x,y) at intersection of perpd and length 
xperp - cenbod(l,l) (pcrpd^in(ditec)); 
yperp = cenbod(l,2) - (perpd*cos(direc)); 

34 lest if foot is inside or outside radius 

radtest = s<p1((foot(l,2) • ceribod(U)r2 + (foot(l.l) • ceiibod(l.l)r2); 
if (radtesl > rad) 

% find (x,y) at intersection of ray and are 
xint - xperp - (lenglfa*cos(direc)); 
yint = yperp - (lengih*sin(direc)); 
intcpt - [xint, yint]; 
else 

*/• find (x,y) at intersection of ray and are 
xint - xperp + (1ength*cos(ditec)); 
yint - yperp + (Ienglh*sin(direc)); 
intcpt - (xint, yint]; 
end 
U 

*/• test if intersection is in desired direction 
indir - foot(l,l)*o<^direc) + foot(l,2)*sin(diiec); 
outdir - xint*cos(diiec) + yint*xin(direc); 
if (indir <= outdir) 

dist = sqrt( (yint - foot(l,2))^ + (xint - foot(l,l))^ ); 

•/i 

*/o test if intersection is within are segment 

arefoot - atan2( yint - cenbod(l,2), xint - cenbod(l,l) ); 

narelim = atan2( liiTB>eg(l,2) • cenbod(l,2), limn^l,!) - cenbod(l,l) ); 

parclim - atan2( limpos(l,2) - cenbod(l,2X linipos(l,l) - cenbod(l,l) ); 

if (flnum = 1) 

if (arefoot >- narelim) & (arefoot <- parclim) 
flag - 1; 






«od 

ebe 

tf(«firac>0) 
if (narclim < 0) 
naidim > narclim * (2*pi); 
end 

if(paictim<0) 
parclim ^ pardim + (2*pi); 
e^ 

if(arcfoot<0) 
a^oot > arcfoot (2*pi); 
end 

if (arcfoot >> naidim) & (arcfoot <*= parclim) 
flag- 1; 
end 

elae %(direc<0) 
if(narclim>0) 
narclim - narclim - (2*pi); 
cod 

if(parclim>0) 
parclim - parclim • (2*pi); 
end 

if (arcfoot > 0) 
a^oot arcfoot • (2*pi); 
end 

if (arcfoot >= narclim) & (arcfoot <- parclim) 
flag - 1; 
end 
end 
end 
end 
end 
end 

if (flag— 1) 
intcpt-Q; 
dist-limit; 
flag-0; 
cod 

% Title; arcint2.m 
% Inputs; (l^,4,Sf6) 

% Outputs; [1.2^,4] 

%Puipose; This Midlab function determines the inteisection of a directed 
% my and an arc. 

% 

function [xint,yim,di$t41ag] - aTcitit2(rx^/theta^,ycjna<i) 

% 

% rad - radius of ciicle(arc segment) 

%xc/yc- coordinates of center of circle 
% rx/iy = ray eoo^)onents 
% itheta - direction of interest 
% xint/yint - arc segment intercepts 
% dist - distance between rx/iy xint/yint 
%flag- 1 ifinteicept found; flag-Oifno intercept found 
% 

patclini-30; 

Darclim-30; 
rCjeta-(itheta*pi/l 80); 

% 

d-(yo-ry)*cos(itheta)-(xc4x)*sin(t1heta); 

ifd<rad 

lenglh-s(p1(iad'^-d'^); 

elseifdr=-rad 

leoglh=0; 

d-rad; 
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else 

xint-D; 

yint-Q; 

dist-Q; 

flagrO; 

end 

xpeiy“x c -Kd*sin(rtlieU)X 

yperp-yo-(d‘coe(rtheto)X 

nMkest-iqrt((ry-ycy^+(ix-xcy^); 

ifradtesOnd 

xiut -3q)eip-<l«°8th*oos(rthett)); 
yi ut- yi»eip-(lenglh*iin(itheU)); 
ebeifra<kest<tad 
xiiit-xpenH<length*cas(ithe(s)); 
yint ' ^ypetp * (lengili*sin(ttliets)); 

% elseif iidtest=nd 
else 
fUg-0; 
end 
% 

Htest ifintenectioa is in desired direction 

xpas>Tad*sin(parclim*pi/180>i-xc; 

ypos=rad*cas(parclim^i/l 80yi-yc, 

stcpos»aIan2(ypos-yinMpos-xiiit); 

xneg°Tard*sin(-narcto*pi/l 80)+xc; 

yneg-rad*oos(-fiarclim^l80>+yc; 

si<cDep'atsii2(yint-yneg,xiiit>xneg}; 

testpos>:q>os*cos(srcpos>^ypas*sin(srcpas); 

testiieg-»>eg*oos(sraie^yneg*sin(arcneg); 

testislp~xiiit*oos(arcpas>)-yint*sin(srcpa8); 

te3tintii>xiiit*cas(srciie^+yint*siii(srcneg); 

if (tesiintp<=testpos) A (testintii>'*testnes) 

dist-sqtt((yiiit-ryy^-t<xint-txy^); 

else 

xint°=Q; 

yint-Q; 

dist-Q; 

flsg-O; 

end 

% Title; b2wtntisjn 
%IiVuts: (U.3.4,5.6.7,8^) 

% Outputs: [1] 

%Putpose: This function ttansfanns body coordinates to world coordinates. 

^, .*««*.«.*.***•*.«***«»**•*«*•«•*•*«**..*.•••****••*••••*••**»*•*•*•» 

% 

function [world] = b2wtrans(plii, theta, psi, xtrans, ytrans, ztrans, >d>, yb. d>) 

% 

% phi - rotation of {B} about the Xw-sxis; 

% theta - rotation of {B} about the Yw-axis; 

*/4 psi = rotation of {B} about the Zw-axis; 

% xtrans - X-axis translation of {B} with respect to {W}; 

% ytrans - Y-axis translation of {B} with respect to {W}; 

% ztrans-Z-axis translation of (B) with respect to {W}; 

% xb = known Xbody coordinate; 

% yb - known Ybody coordinate; 

% zb - known Zbody coordinate; 

% 

% construct the translation vector, the body vector, and the rotation matrix 
trans = [xtrans, ytrans, ztrans]'; 

body = (]d),ybAlT; 

rotate = [cos(psi)*cos(dieta) cos(psi)*sin(theta)*sin(phi)-sio(psi)*cos(phi) cos(psi)*sin(theta)*cos(phi>+sin(psi)*sin(phi); 
sin(p6i)*o^theta) $in(psi)*si^theta)*sin(phi>Hos(psi)*oos(phi) sin(psi)*sin(thela)*cos(phi>cos(psi)*sin^); 
-sin(tli^) c^th^)*sin(idu) cas(th^)*cos4)hi)]; 

•/. 
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% buUd the bo<iy^warid iMnogeoMiiS matrix 
BVrr ~ [route tiaiia;0 0 01]; 

% 

% find the reoihant 4X1 vector of cquivalcat WKirld coordmatea 
TworW-=BWT»body; 

% 

S strip off the (x,y^) coonliiiatea 

world - |Tw«rld(l.lX Twaftd(2.1X Twald(3,l)]; 

% Title; cydouQjn 
Hlqiutt: radius a, stride d 
% Outputs; 

%Purpose; This Matlabcalculates ase^nentofacycloid and eraphs it. 
% 

% 

clear 

dc 

clg 

ain>= 36.31; HinputCStride; '); 
dtheU - inpiit(TiewFoot: J, 
a « ain/(2*piX 
t» 0;a/200;2^; 
x = a»(t-sin(t)X 
y = a‘(l-cos(t)); 
plot(x.y) 
grid 

titl«(X)ydoid Foot Motion') 
xlabelOStride • cm*) 
ylabelCHeigbt • an!) 
hold on 

theU =t(l,l)*pi/180; 

dt = 2*dthda*pi/180; 

xnew = a*((th^-)dt) - siii(theta+dt)); 

ynew = a*(l • oos(theU-Hlt)); 

plot(xnew,ynew,'g+-') 

holdo£f 

*/.TiUe; el3.ffl 
% Inputs; 

% Outputs; 

% Purpose: This Matlab program calculates an elliptical foot path. 

% 

% T^VarZ + y''2/b^ =1, a>b 
% 

hold off 
cic 
clg 
clear 

pfic”inputCEnter present foot-X; ■); 
pfy=0; %inputCEi>ter present foot-Y; y, 
dbc^inputCEnter desii^ foot-X; y ,. 
dfy=0; 3^put(Tnter desired foot-Y; y, 
a»(sqn((dfy-pfy)^+(dfit-pfic)^))/2; 

%3et aspect 
b=20; 

x=-a;a/20;a; 

ifpfx<dfx 

xreal=pfx;(2*a)/(length(x>l);dfic; 

else 

xreal”p6c;-(2*ay(length(x)-l);dfic; 

end 

elpse=n; 

forp=l;length(x) 
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y-b»«irt(l-x(l4>r2/«'^); 

el|Mc-idpw.xi«U(l4>) y]; 

end 

axii([0 elpuOcnglliCxXIHSO elpie(U>20 ISO]) 

l>lol(dpM(:,l).eh»M(:^XW 

tiUe(nii|itic>l Foot Pith') 
xUbelCX ooofdiiiitei, (cm)) 
ylibel^ eoordiiiatci, (cm)') 
hold on 

plot(pficj)fy,'oc8) 

plot(dfic,dfy,'oc8) 

grid 

% Title: ellipsejn 
Si Inputs: (1^) 

SiOu^Nits: [1,2^] 

SiPuipose: This Matlahfiinctioacilculates in ellipticil foot path using 
Si the known foot position and the desired amount of foot 

Si movement in the Xdirectioa 

Si 

Si x''2/a''2 + y'W2 - 1; a > b 
Si 

fonction [elpse, aspect, inc]« ellipsefpfic, d&) 

Si 

pfy-0; 

dfy-0; 

a - (sqrt((dfy-pfyy2 + (dfit-pfity'2)y2; 

Si set aspect 

b > 20; Si 20cm is nuudmum height (Z) or Link3 will overlap Link2 

c ~ s(pt(abs(a''2 • b^)); Si absolute because b>a 

inc = -a:a/20:a; Si actiial nunfoer of increments will probably be >300 

ifpfic<dfo 

xreal >> p£c(2*aV(]englfa(inc)-l):d6(; 
else 

xreal ~ pfic-(2*a)/(lengib(inc>l):dfic; 
end 

elpse - Q; 

forp= I:lenglh(inc) 
y - b*sqtt(l - ino(l.py'2/a'^); 
elpse = [elpse-,xreal(l,p) y]; 
end 

aspect = c/s; 


^ *•«••*•*•«•••«*«*••*••«***•*«••*•**•*•*•*•*•«**«•**•***•*•»**•***••** 

Si Title: invldn.m 
•/ohqwts: (1,23) 

SiOufouts: [133.4] 

SiPurpose: This Matlab fonction provides the inverse kinematics of the 
Si AquaRobot for a given foot location (px,py,pz) and a chosen 
% teg number (1-6). 

Si 

fonction [theta0d,tlietald,theta2d,tiieta3d]^vkin(le&px,pyj>z) 

Si 

Si tbetaO'tf^degiees; without '(f^radians 
Si 

a0=37.5; 

al=20; 

a2=50; 

a3=100; 

Si 

ifleg=l 
theta0=0*pi/180; 
elseifleg*“2 
theta0=-60*pi/l 80; 
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elscifleg->3 

theU0--120*pi/180; 

ebeifleg=««M 

thdaO--18O*pi/lS0; 

ebciflig '• 3 
theU0>-240*pi/180; 
ebeifleg->>6 

theU0--300*pi/180: 

else 

1faet*(M>*pi/lS0; 

end 

thcUOd^thetaO* 1SO^; 

% 

bl“iq^(px-»0*co«(theUO)y^+(py^d)*siii(thetjO)y^); 

ci=(p*^+py^-«o^-bi'^y(2*«o*bi); 

tlieUl(t-ataii 2 (s<|rt(l<l^Xcl)* 180 /pi; 

% 

b2=bl-el; 

b3“iqft(b2'^+ia^X 
c2=(«2'^+M'^-i3^y(2*a2»b3); 
theU2iB-ataii2<-sqrl( l-c2^),c2)-aUii2(pz,b2); 
theu2*ataii2(s^l-c2^Xc2yitaii2(pz.b2X 
if(theU2<-106.6*pi/l*0)&(theta2>=-73.4»pi/180) 
theta2<Hlieta2*180^; 
else 

thete2^^heta2iii; 

theU2d°°theU2*180/pt; 

end; 

% 

c3-(b3^-)i2'^-i3'^y(2*s2»s3); 
tbeU3m*‘sun2(-eqrt(l-c3^),c3); 
tbeta3~ataD2<s^l-c3'^Xc3); 
%if(theU3<«156.4V180)&(theU3>=-23.6»pi/180) 
% tliets3>4iieta3m; 

% tbeti3<HheU3*180/pi; 

%ebe 

% theU3d~tlieta3*180/pi; 

•Aeoi; 

if thets3>=0*180/pi 
theU3d-tbeU3*480/pi; 
else 

theU3^'tlieU3in; 

theU3d=aieU3*180/pi; 

end; 


% Title: jcoordm 
% Inputs: 

^Oii^wts: 

% Purpose: This Matlab program cbedcs the kinemsticsoflheAqusRoboL 
% Joints Zero, One, Two, and Three are used. 

% 

Idelc 
diary c 

angleO>‘itq)Ut(Tnter Leg angle (0,60,120,180,240,300): y, 
anglel=itg)UtCEnter Hip angle : y, 

angte2'‘input(Tnter Kneel angle : '); 

angle3°°iiiput(Ti)iter Knee2 angle : y, 

diary off 
% 

angle0=angle0*pi/l 80; 
anglel=anglel*pi/180; 
angle2’‘angle2*pi/180; 
angle3>'angle3 *pi/l 80; 

•/. 

aO-37.5; 
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* 1 - 20 ; 

•2=30; 

a3~100; 

% 

TBO-{c<n(MigteO) -ciii(aiigleO) 0 O', 
siii(iii^0) oai(aiigieO) 0 0; 
0010 ; 

0001 ]; 

% 

T01>[cat(aiigt6l) -ctii(aiiglel) 0 «0; 
siii(aiit^l) coi(aiiglel) 0 0; 
0010 ; 

0001 ]; 

% 

diaiy c 

H1P-TB0*T01 
pause 
diaiy off 
% 

T12>[coa(aiig|e2) -sin(aiigle2) 0 al; 

0010; 

-ain(aiigle2) -caa(aii^e2) 0 0; 

0001 ]; 

% 

diaiyc 

KNEE1-TB0*T01»T12 
pause 
diaiy off 
% 

T23-[oos(aog|e3) -siiiCaiigteS) 0 a2; 
siii(an^e3) oos(sngle3) 0 0; 

0 010; 

0001 ]; 

% 

diaiyc 

KNEE2-TBO»T01»T12*T23 
pause 
diaiy off 
% 

T34=[10 0a3; - 
010 0 ; 

0 010 ; 

0001 ]; 

% 

diaiyc 

FOOT=TBO*T01*T12»T23*T34 
diaiy off 


«^ .**«*«««*«*«*****«**********«*«*»*«.«**«*«**•***••*•••*••««•«•* 

% Title: jkin0123jn 
% Inputs: 

% Outputs: 

%Puipose: This Matlab program checks the kinematics of the AquaRobot 
% Joints Zero, One, Two, and Three are used. 

s^*************************************************************** 

% 

!delc 

diaiyc 

angieOHnputfEnter Leg angle (0,60,120,180,240,300): y, 
anglel^HnputCEnter Hip angle : '); 

angle2~ii9Ut('Enter Kneel angle : J, 

angle3=ini>ut('Enter Knee2 angle : y, 

diaiy off 
% 

angle0~angle0*pi/180; 

anglel’'angtel*pi/180; 

angle2°angle24>i/180; 
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angjc3-aogle3*pi/180; 

% 

•0-37.5; 

• 1 - 20 ; 

•2-50; 

V, 

TBO-(otM(«igtcO) •«iii(«igte0) 0 0; 
smC^iigtcO) co((^iigM) 0 0; 

0010; 

0001 ]; 

% 

T01-(ca«(^iiglel) -onC^nglcl) 0 •O; 
sin(^ii^l) co^Migiel) 0 0; 

0010; 

00 01 ]; 

H 

dUry c 

HIP-TB0*T01 

puise 

£aryoS 

% 

T12-(ooi(^iig|e2) -•ill(•llgie2) 0 al; 

0010; 

-«iii(aii^e2) •ca*(aiigle2) 0 0; 

0001 ]; 

•A 

diaiyc 

KNEE1-TBO*T01*T12 
pause 
diaiy off 
% 

T23-[cas(angle3) -siii(aiigle3) 0 ^2; 
sin(aiig|c3) oo^angleS) 0 0; 

0010; 

000 1 ]; 

% 

diatye 

KNEE2-TB0*T01*T12»T23 

pause 

diaiy off 

% Title: jkiiil23.in 
S Inputs: 

SOu^iuis: 

^Purpose: This Matlab program checks the kinematics of the AquaRoboL 
% Joints One, Two, and Three are used. 

^,««*««**.*.««.*«************************************»*****»** 

% 

ideic 

diaiyc 

anglel-inputCEnter Hip angle : Of 

angle2-iiqiut('Eiiter Kaeel angle : *}; 

angle3-input('Eiiter Knee2 angle : *); 

diaiy off 

•/. 

anglel-anglel*pi/l 80; 
ang|e2=angle2^180; 
angles-angle3*pi/180; 

% 

•0-37.5; 

al-20; 

a2=50; 

•3=100; 

% 

T01=[cos(anglel) -sin(anglel) 0 aO; 
sin(anglel) cos(anglel) 0 0; 
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T12«[ciM(aiigle2) -f>n(aiigle2) 0 al; 

0010 ; 

■aiii(aiigte2) -oat(iagle2) 0 0; 

0001 ]; 

duty C 

KNEE1-T01T12 

^txyoS 

T23-[oo((angle3) -aiii(aiigte3) 0 a2; 
siii(aiig|c3) oa<aiigie3) 0 0; 

0010 ; 

0001 ]; 

diaiy c 

KNEE2-T01»T12*T23 

pause 

diary off 

% Title: kiii2.m 
% Inputs: (UA4) 

% Outputs: [1^.4] 

HPuipose: This Matlabfimctiaa provides the kinematics of the AquaRobot 
% fora given set of joint angles. Uses new LINK lengths. 

% 

fonction [HipJCneel JCnee2,Foot] • kin2(theta0,thelal,theta2,theta3) 

% 

angle0"theta0*pi/l 80; 
anglel ^thetal *pi/l 80; 
angle2>theta2*pi/180; 
angle3>theta3*pi/180; 

% 

a0=37.5; 

al-21.5; 

a2-52; 

a3>101; 

% 

TB0=[oos(angle0) -sin(angle0) 0 0; 
sin(angle0) co^angleO) 0 0; 

0010; 

0001 ]; 

% 

T01>[co$(anglel) -sin(anglel) 0 aO; 
sin(anglel) co^anglel) 0 0; 

0010; 

0001 ]; 

% 

H=TB0*T01; 

% 

T12=[cos(angle2) -5in(angle2) 0 al; 

0 010; 

-sin(an^e2) -cos(angle2) 0 0; 

0001 ]; 

% 

K1=TB0*T01*T12; 

% 

T23=[caf(angle3) -sin(angle3) 0 a2; 
sin(angle3) cos(angle3) 0 0; 








K2-TB0»r01*T12»T23; 

% 

T34-{100»3; 

0100; 

0010 ; 

0001 ]; 

% 

FT-TBO»T01*T12*T23*T34; 

S 

Hip-(H(1,4)H(2,4)H(3.4)1; 

Kiieel-[K1(1,4) Kl(2.4) Kl(3.4)]; 

Knec2-[K2(1.4) K2(2.4) K2<3,4)]; 

Foot-[FT(l,4) FT(2.4) PT(3,4)]; 

^ ••*••••••••••*•••••»•*••*•*««••••*•••••••••••••••••••••••••*••••••••• 

Slltle: nMUcdm 
Hbpiitt: (U3,4.3,6.7.W) 

%Oul])iib: [1] 

HPufiraae: ThttMatUbiiaiclioafinditlieiDudmuinsIndepoMtUefar 
% AquaRobot, given an ariMtruydirectiaa as an input 

U 

function [niaxdiat]~inanl(fbotl,foot2,foat3^oot4/ootS/oot£,<iirec,oeid>od,lri) 

% 

S maxdist maximum foot movement in the direction requeited 
% fiiotl jootti - (x,y) coordinatei of fiMt 
H direc - diiectian of ntterert 
S oenbod • (x,y) coordinates of the center of the body 
34 tri • tripod oianbcr 0 (lege 14,3) or number 2 (legi 2,4,6) 

S 

% ••••• MUST CONVERT (X,Y,Z)WORlD COORDINATES TO BODY COORDINATES HERE ••••• 
S 

34 initializatiooa here 
34 

inrad •> 37.3; 34 inner ndius 

outrad > 178.2107; 34 outer radius 
34 

seglpin ~ [36.8686,6.8324]; 34 inside end^int of positive segment #1 
seglpout • [160.2049,78.0606]; 34 outside en^int of positive segment #1 
seglnin > P6.8686, •6.8324]; % inside endpoint of negative segment #1 
seglnout - [160.2049, -78.0^]; 34 outside endpoint of negative segment #1 
34 

segSpin » [12.3,334333]; 34 inside endpoint ofpositive segment #2 
seg2pout ■> [12.3,177.7718]; S outside endpoint of positive se^nent #2 
seglnin *• [24.3686,28.3030]; 34 inside csidpoint of negative segment #2 
se^nout«[147.7049,99.7112]; 34 outside endpoint of negative segment #2 
34 

seg3pin > [-24.3686,28.3030]; % inside endpoint of positive segment #3 
seg3pout > [-147.7049,99.7112]; 34 outside endpoint of positive segment #3 
seg3nin - [-12.3,33 3333]; 34 inside endpoint of negative segment fO 
seg3nout - [-12.3,177.7718]; 34 outside endpoint of negative segment #3 
34 

seg4pin > [-36.8686, -6.8324]; 34 inside endpoint of positive segment #4 
seg4pout • [-160.2049, -78.0606]; 34 outside endpoint of positive segment #4 
seg4nin > [-36.8686,6.8324]; 34 inside endpoint of negative segment #4 
aegtnout»[-160.2049,78.0606]; 34 outside endpoi n t of negative segment #4 
34 

seg3pin~ [-12.3,-333333]; % inside en^Mnnt (^positive segment #3 
seg3pout - [-12.3, -177.7718]; 34 outside endpoint of positive segment #3 
seg3nin • [-243686, -28.3030]; 34 inside endpoint of negative segment #3 
seg3nout • [-147.7049, -99.7112]; 34 outside endpoint of negative segment #3 
34 

seg6pin = [24.3686, -28.3030]; 34 inside endpoiM of positive segroem #6 
seg6paut = [147.7049, -99.7112]; 34 outside endpoint of positive se gment #6 
seg6nin = [12.3, -33.3333]; 34 inside endpoint of negative segment fi6 
seg6nout ~ [12.3, -177.7718]; 34 outside ent^intofnegativesegnient #6 
% 
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while (tri “ 0) 

%tertfoatl in kg 1 woriupeoe 

ft-1; 

[inlciit, dull. flag]-arant(footl, direc, ceobod, outnd, seglnxit, leglpout, ft); 
if(fUg“ 1) 

(hsU^distl; 

end 

(iolcpt, distl, flag]-aiGinl(ft)a(l. doec, cenbod, imd. seglnin. seglpin. ft); 
if(flig“ 1) 

(hsU - dittl; 
end 

[inlGiit, dikl, jBag]-wgml(fi)otl. dine, s^lpni. seglpout); 
if(fl*g— 1) 
ihfU-diitl; 
end 

[intept, dull. flag]*tegiiii(footl, dine, seglnin, seglnout); 

if (flag—1) 

<hiU~distl; 

end 

% 

% test foot3 in kg 3 workspace 
ft-3; 

(mtept, dist3, {k^arctnt(ft>oC3, dine, cenbod, outnd, seg3nout, segSpout, ft); 
if (flag — 1) 

ead 

[inept, diets, flag]-arcin(ft>at3, diree, eenbod, inrad, segSnin, segSpin, ft); 

if (flag a» 1) 

a dist3*, 
end 

[inept, diets, flag]°>eegin(foot3, diree, segSpin, segSpoutX 
if (flag" 1) 
a dist3a 
end 

(inept, diets, flagJ-seginffootS, diree, segSnin. seg3nout); 
if (flag—> 1) 
dietb^distS; 
end 
% 

S test foots in leg S wofkspace 

ft* J; 

[inept, diets, flag]*arcint(footS, dine, cenbod, outrad, segSnout, segSpout, ft); 
if(flag“l) 

— dist5; 
end 

[inept, diets, flag]=arctn(foatS, dine, cenbod, inrad, segSnin, segSpin, ft); 
if(flag” 1) 
ihstc = diets; 
end 

[intqit, diets, flag]~segin(foatS, diree, eegSpin. segSpout); 
if(fl^» 1) 
distc = diets; 
end 

[intept, diets, flagl^'seginffoatS, diree, segSnin, segSnout); 
if (flag =1) 
dieted diets; 
end 
% 

inaxdist - min( 0 iin(dieta, distbX dietc); 

% 

break; 

end % this ends tripod 0 testing 
% 

while (tri =“ 2) 

% test fool2 in leg 2 workspace 
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ft-2; 

[ialG|il. flag]>ananl(fi>at2. direc. ocobod, outrtd, <eg2iiout, ieg2paut. ft); 
1) 

dlUrta * dtlt2*, 

end 

[iniqpt, dtit2, flag]*'a(cinl(fi>ot2, direc. cenbod, imd. segZnin, le^pitt, ftX 
1) 

distft * diit2» 
cad 

(mtcpt, diit2. flag]-seginl(fi>ot2, direc, seg^pin. ceg^pouty, 
if(fUg" 1) 

dli«ta ai dtft2i 

cad 

[iaiqit, diit2, flag]-iegiat(foat2, direc, le^nn, tetflaout); 
if(flag“- 1) 

(Usts~diit2; 

end 

% 

% test fi>ot4 in leg 4 wotkijnce 
ft-4. 

[iatqit, dist4. fUg]>erciitt(foot4, direc, ceabod, oiitnd, cegdaout, teg4pout, ft); 
if (flag =1) 
distb~diit4; 
end 

[intcpi, dist4, ileg)>areint(foot4, direc, cenbod, innd, seg4oin, ieg4pin. ftX 
if(fUg=.l) 

(fisib = diat4; 
end 

[intcpt, dist4, flag|-segint(fi>ot4, direc, >eg4pin, seg4poiit>, 
if(fleg=" 1) 

(Uftb ■* didV, 
end 

(inl^ diA4, iUg]>*iegint(foo«4. direc, aeg4nin. legdnout); 
if(fl»g=“ 1) 

^xtb>diet4; 

end 

% 

H teit fooCd in leg 6 woifcspece 
ft “ 6 ; 

[intcpl, dist6. iIag]>arcint(fbot6, direc, cadxxi, outred, legdnout, seg6pout, ft); 
if (flag — 1) 

a dlSt6a 

end 

[intc])t, dist6, llag]»*rcint(foot6, direc, cenbod, innd, s^6niii, seg6pin, ft); 
if (flag—1) 
m dut6^ 
end 

[iniqpt, dist6, flag]>segint(foot6, direc, segfipin, segfipout); 
if(ll*g“l) 

end 

[intcpt, dist6, flag]>Kgint(foot6, direc, seg6nin, segdnout); 
if (flag = 1) 
diatc>dist6; 
end 
% 

maxdist > inin( inin(dista, distb), distc); 

% 

break; 

end % this ends tripod 1 testing 


./.***••**•*•«***««•****••*•****••««•••••**«•**•**••*•••*•**•*•. 

% Title: niaxd2S.m 
•/iligwts; (1,23,4.5,6.7,8,9) 

% Outputs: [1] 

%Puipose: This Matlab function finds the maximum stride possible for 
% AquaRobot, given an aibitnry direction as an input 
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TliisfiiiictiooafortfaeS} on footpad. 




% 

% 

% 

function [maxdut]^niaxd25<footljroot2.foot3,fi>ot4/oot3/oot6,difec,ca>bo<l,th) 

% 

% maxdist = maxiinum foot movemeot in the directioa requested 
% fixitl ifootfi - (x,y) coordinates feet 
% diicc = diiectioa of interest 
% cenbod ^ (x,y) coordinates of the center rtf'the body 
%tri-tripod number 0(1^9 13.S) or number 2 (legs 2,4,6) 

% 

% ••••• MUST CONVERT (X,Y,Z)WORLD COORDINATES TO BODY COORDINATES HERE 
% 

inhialiTations here 

% 

inrad 37.S; % iimer radius 

outrad= 178.2107; % outer radius 
% 

seglpin ~ P6.8d86,-d.8524]; H inside erx^xnnt of positive segment#] 
seglpout - [160.2049, -78.0606]; % outside endpoint of positive seffoeal #1 
seglnin = [36.8686,6.8324]; % inside endpoint of negative ttffoeA #1 
seglnout > [160.2049, 78.0606]; % outside endpoint of negative se^nent #1 
•/. 

seg6pin = [12.3,33.3333]; inside endpoiid of positive sequent #2 

3eg6pout = [12.3,177.7718]; % outside endpoint of positive segmerd #2 
seg6nin - [24.3686,28.3030]; 34 inside endpoint of negative se^nent #2 
seg6nout = [147.7049,99.7112]; % outside endpoint of negative se^nent #2 
% 

seg3pin - [-24.3686,28.3030]; 34 inside endpoint of positive segment #3 
seg3pout = [-147.7049,99.7112]; 34 outside endpoint of positive se^noat #3 
seg3nin > [-12.3,33.3333]; 34 inside endpoint of negative segment #3 
seg3nout - [-12.3,177.7718]; 34 outside em^wint of negative segment #3 
34 

seg4pin = [-36.8686,6.8324]; 34 inside endpoint of positive segment #4 
seg4pout > [-160.2049,78.0606]; 34 outside endpoint of positive segmead #4 
seg4mn [-36.8686, -6.8324]; % inside endpoint of negative segment #4 
seg4nout > [-160.2049, -78.0^]; % outside endpoiid of negative segment #4 
% 

seg3pin> [-12.3,-33.3333]; 34 inside endpoiruafpositive segment #3 
seg3pout" [-12.3, -177.7718]; 34 outside endpoiid of positive segment #3 
seg3nin - [-24.3686, -28.3030]; 34 inside engird of negative segment #3 
seg3nout = [-147.7049, -99.7112]; 34 outside endpoiid of negative segmesd #5 
34 

seg2pin = [24.3686, -28.3030]; 34 inside endpoint of positive segment #6 
seg2pout (147.7049, -99.7112]; 34 outside endpoiid of positive segment #6 
seg2nin = [12.3, -33.3333]; 34 inside endpoint of negative sepnent #6 
seg2nout = [12.5,-177.7718]; 34 outside endpoint of negative segment #6 

3. 

flag “ 0; 
while (tri “= 0) 

34tesl footl in leg 1 woilcspace 
ft - 1. 

(mlcpt. distl, ilag]-arciiit(footl, direc, cenbod, outrad, seglnout, seglpout, ft); 
if (flag == 1) 
disu = distl; 
end 

(intept. dist I, flag]=arcint(fooll, direc, cenbod, inrad, seglnin, seglpin, ft); 
if(nag==l) 
dista == distl; 
end 

[intept, distl, flag]=-segiiit(footl, direc, seglpin, seglpout); 
if (flag == 1) 
dista = distl; 
end 

[iidcpl, distl, flag]=segiid(footl, direc, seglnin, seglnout); 
if (flag =1) 

(hsta ■= distl; 
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end 

% 

H tcA foots in leg 3 woilcspace 
ft«3; 

[intciit, diets, flag]~wcint(foat3, dtiec, cenbod, outnd, seg3nout, legSpout, ft); 
if (flag =1) 
distb “ dist3; 
end 

[intqit, diets, fUg]°afGint(foot3, direc, cenbod, inrad, eegSnin, eegSpin, fl); 
if(fleg“ 1) 

riirth 3B distS^ 

end 

[intcpt, diets, fIeg]>segint(foot3, direc, segSpin, eegSpout); 
if (flag =1) 
dtsCb ^ dist3^ 
end 

[intcpt, diets, flag]=segiid(foot3, direc, segSnin, segSnout); 
if(flag= 1) 
dislb = diets; 
end 

% teet foots in leg S woricepace 

11 = 5; 

[intcpt, diets, flag]=arcint(footS, direc, cenbod, outnd, eegSnout, eegSpout, ft); 
tf(flag= 1) 

K diets; 

end 

[intcpt, diets, ilag]°=atcint(footS, direc, cenbod, inrad, eegSnin, segSpin, ft); 
if(flag*= 1) 
dietc~ diets; 
end 

[intcpt, diets, flag}=5egint(footS, direc, eegSpin, eegSpout); 
if (flag “= 1) 
dietc^ diets; 
end 

[intcpt, diets, flag]'=5egint(ft>otS, direc, eegSnin, eegSnout); 
if (flag =1) 
dietc>=diaC; 
end 
% 

niaxdiet niin( min(dista, dietb), dietc); 

% 

break; 

end % this ende tripod 0 testing 
% 

- - .I - - . - -- ■ 

while (tii = 2) 

% test foot2 in leg 2 workspace 
ft»2; 

[intcpt, dist2, flag]=arcint(fbot2, direc, cenbod, outrad, seg2iK)ut, seg2pout, ft); 
if (flag — 1) 
dista dist2; 
end 

[intcpt, dist2, flag]=arcint(foot2, direc, cenbod, inrad, eegSnin, eegSpin, ft); 
if(flag==l) 
dista = dist2; 
end 

[intqit, dist2, flag]^=segint(foot2. direc, segSpin, eegSpout); 
if(flag=l) 
dista = dist2; 
end 

[intcpt, distS, flag]=segint(foot2, direc, eegSnin, segSnout); 
if(flag=l) 

(hsta = dist2; 
end 
% 

% test footd in leg 4 wwkepace 
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11*4; 

(iniqit, dixt4, lUg]*arcml(fi>ot4, direc, ccabod. outnd, ic84nout, seg4poiit, ftX 

didb * diit4; 
eod 

[inicpt, dist4, flag]=arciiit(fiiot4. dtrw, ccnbod. imd, >eg4iiiii, seg4ptii, ft); 
if (flag “ 1) 
iflstb * <liat4; 
end 

(intcpt, dist4, flag]*segiiit(foot4, direc, seg4pin, segdpout); 
if(flag==l) 

(Ustb*dist4; 

end 

[intqpt, dist4, flag]*segint(foot4, direc, seg4ain, wgdnout); 
if (flag = 1) 
di$tb*dist4; 
end 
% 

44 test footd in leg 6 workspace 
ft*6; 

[intqit, distd, flag]*arcinl(foat6, direc, cenbod, outrad, segdiKNit, segdpout, ft); 
if(flag>*l) 
s dist6; 
end 

[iitfcpt, distd, flag]*arcint(foot6, direc, cenbod, inrad, segdnin, segdpin, ft); 
if (flag = 1) 
as distS*, 
end 

[intcpt, distd, flag]-segint(foot6, direc, segdpin, segdpout); 
if (flag =1) 
distc*dist6; 
end 

[bnept, distd, flag]*segint(foot6, direc, segdnin, segdnout); 
if(flag= 1) 
distc * distd; 
end 
% 

maxdist * mia( inin(dista, distbX distc); 

% 

break; 

end %this ends tripod 1 testing 


% Title: maxd4S.m 
% Inputs; (1,2,3,4,5,6,7,8,9) 

% Outputs: [1] 

%Putpose; This Matlabfimction finds the maximum stride possible for 
% AquaRobot, given an arbitrary direction as an input 
% TUs function is for the 43 cm fooqnd, 

0 ,^ «***«•**«*»«•••*****»******•****•**•«••«***•*•*•****•.*•*•***••*** 

% 

function [maxdist]=maxd4S(footl/oot2/oot3/oot4/oot3/oot6,ditec,cenbod,tri) 

% 

44 maxdist = maximum foot movement in the direction requested 
% footl :foot6 = (x,y) coordinates of feet 
% direc = direction of interest 
% cenbod * (x,y) coordinates of the center of the body 
% tri * tripod number 0 (legs 1,3,3) or number 2 (legs 2,4,6) 

% 

% ••••• MUST CONVERT (X,Y,Z)WORLD COORDINATES TO BODY COORDINATES HERE 
% 

% initializations here 
% 

inrad * 43.0; % inner radius 

outrad = 149.27; % outer radius 
% 

seglpin * [43.0,0.0]; % inside endpoint of positive segment #1 
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wglpout • (139.0446,54.2967]; S outside eodpoiot of poedivc scgnieat #1 
■eglnin » [45.0,0.0]; 54 ioeide endpoint of negative ae^neot #1 
leglnout - (139.0446, -54.2967]; H outside cadpoud of n^stive segment #1 
% 

seg^pin-[22.5,38.9711]; 54 neidecndtiointofpoailive segment 82 
seg2pout - [22.5, 147.5645]; 54 outside endpoint of positive tffoaf #2 
seg2nm-[22.5,38.9711]; 54 inside endpoint ofneptivesc^tieat #2 
seg^nout > [116.5446,93.2678]; % outdde endpoint of negative segment 82 
% 

seg3pin-[-22.5,38.9711]; 54 ins^ endpoint ofpositivesepnent 83 
segSpout > [-116.5446,93.2678]; 54 outside endpoint of positive segment 83 
aeg3nin-[-22.5,38.9711]; 54 inside endpoint of negative segment #3 
segjnout - [-22.5,147.5645]; % outside endpoint of negative seffnent 83 
54 

segtpin = [-45.0.0.0]; 54 inside endpoint (^positive segnent 84 
seg4pout • [-139.0446, -54.2967]; S outside endpoint of positive segnent 84 
seg4nin ~ [-45.0,0.0]; % inside endpoint of negative sequent 84 
segtnout - [-139.0446,54.2967]; 54 outside en^int of negative sepnent 84 
54 

seg5pin = [-22.5, -38.9711]; 54 inside endpoint of positive segment 85 
aeg5pout • [-22.5, -147.5645]; 54 outside endpoint positive segment 85 
seg5nin > [-22.5, -38.9711]; 54 inside endpoint of negative segment 85 
seg5nout - [-116.5446, -93.2678]; 54 outside ent^xiint of negative segment 85 
54 

3eg6pin > [22.5, -38.9711]; 54 inside endpoiid of positive se^nem 86 
seg6pout= [116.5446,-93.2678]; 54 outside en^intofposhive segment 86 
segdnin = [22.5, -38.9711]; 54 inside en%>oint of negative segment 86 
segdnout - [22.5, -147.5645]; 54 outside endpoint of negative sepnent 86 
54 

flag'O; 
vdule (In “ 0) 

54testfootl in leg 1 wotfcspace 

ft=l; 

[intept, distl, flag]=arcint(footl, diiec, cenbod, outrad, seglnout, seglpout, ft); 
if(flag==l) 
dista ~ distl; 
end 

[intept. distl, lUg]>*egint(fixitl, diiec, seglpin, seglpout); 
if(flag=l) 
dista = distl; - 
end 

(intept, distl, flag]=segint(footl, diiec, seglnin, seglnout); 
if(flag= 1) 
dista - distl; 
end 
% 

54 test foots in leg3 woiicqnoe 
ft = 3; 

(intept, dist3, flag]=arcint(foot3, diiec, cenbod, outrad, seg3nout, seg3pout, ft); 
if (flag “ 1) 
distb dist3; 
end 

(intept. dist3, flag]=5egiiit(foot3, diiec, seg3pin, seg3pout); 
if (flag = 1) 
distb = dist3; 
end 

(intept, dist3, flag]=segint(foQt3, diiec, seg3nin, seg3nout); 
if (flag == 1) 
distb = dist3; 
end 
% 

% test foot5 in leg 5 woikspace 
ft = 5; 

[intept, dist5, flag]=aTcint(foot5, diiec, cenbod, outrad, seg5iK>ut, seg5pout, ft); 
if (flag = 1) 
distc = dist5; 
end 
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(iKopt, flag]>(eginl(faotS, diicc, segSpin, legSpout); 
if(fliig“ 1) 
m dij|5‘ 

Old 

difl5. flag]-ieginl(fi>atS, direc, segSnin, segSnout); 

1 ) 

A*!/* s dist5; 
end 

Vt 

maxdist - inin( min(disu, distbX dttfe); 

% 

bfcak; 

end % this ends tripod 0 toting 
% 

while (tri ■“ 2) 

% test foot2 in leg 2 woricspscc 
ft -2; 

[inlcpl. dist2, fbg]=arciin(foot2, direc, cenbod, outrsd, tcg2nout, seg2pout, ft); 
if(flsg“ 1) 

SB dist2» 
end 

(intept. dist2, flsg]-segtnt(foot2, direc, seg2pin, seg2pout); 
if (flag “ 1) 

end 

[intept, dist2, flag]>segint(ft>ot2, direc, seg2nin, seg2noiit); 
if(flsg“ 1) 
distJi * dist2; 
end 
% 

% test fi>ot4 in leg 4 woricspscc 
ft = 4; 

[intept, dist4, ilsg]=*srcint(foot4, direc, cenbod, outrsd, seg4oout, seg4pout, ft); 
if (flag =1) 

<hstb>dist4; 

end 

[intept, dist4, llag]«segint(foot4, direc, seg4pin, seg4pout); 
if (flag — 1) 
distb = dist4; - 
end 

[intept, dist4, fUg]"segint(foot4, direc, seg4ain, $eg4nout>, 
if(flag== 1) 
distb = di$t4; 
end 
•/. 

% test foot6 in leg 6 workspace 
ft = 6; 

[intept, dist6, flag]=arcint(foot6, direc, cenbod, outrad, seg6iiout, seg6pout, ft); 
if(flag=l) 
distc = dist6; 
end 

[intept, dist6, flag]=segint(foot6, direc, seg6pin, seg6pout); 
if (flag—1) 
distc = distd; 
end 

[intqtt, dist6, flag]=segint(foot6, direc, seg6nin, seg6nout); 
if(flag= 1) 
distc = dist6; 
end 
% 

maxdist = min( min(dista, distb), distc); 

% 

break; 

end %this ends tripod 1 testing 

./,•«**»****•••*••••*•***•*•***•«••**••••**•*•*»•*•«••*••••*•••••••** 
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% Title: miLm 
Ktaputs: (1A3.4.3,6.7,*.9) 

% Outputs: [1] 

SPuipaee: TtoMetUbfiipction finds the maitimum Bride possible for 
% AquaRol)al,giv«oanaibitniy<hrectiaaassaiapuL 

H TtefimctionisfiirtbeZS cmfiMtpsd. 

H 

fimction [maxdist]-ind(&otl.foot2/oat34bot4,foat3/oat6,ditec,eeribod.tri) 

% 

% msjdiist • nsudmuffl fiMt movement in the dttectiao requested 
S fi)atl:fiMit6 ° (i^y) oooniinates of feet 
% direc - diieGtioo of intefest 
% oenbod - (x,y) coordiiiates ofthe center of the body 
%tri> tripod mmiberO (legs 13,3) or number 2 (legs 2,4,6) 

% 

iind>'37.3; %iiiDer radius of wotkspsoe 
outrad = 178.2107; % outer radius of woricspace 
% 

seglnin ~ [36.8686, -6.8324]; % inside endpoint positive segment #1 
seglnout - [160.2049, -78.0606]; S outside endpoint of positive se^nent #1 
seglpin > [36.8686,6.8324]; 36 inside endpoint of negative segtneid#! 
seglpout = [160.2049,78.0606]; 36 outside endpoint of negative segment #1 
36 

seg6nin »[12.3,-333333]; 36 inside en^iointi^positive sepnent# 
seg6nout [12.3, -177.7718]; 36 outside endpoint of positive segment # 
aegSpin ~ [24.3686, -28.3030]; 36 inside endpoint of negative aegment # 
segdiMut - [147.7049, -99.7112]; % outside en^int c £negative se^nent # 

36 

aeg3oin - [-24.3686, -28.3030]; 36 inside endpoint of positive aegment # 
seg3naut > [-147.7049, -99.7112]; 36 outside endpoint of positive segment # 
segSpin ~ [-12.3,-33.3333]; 36 inside endpoint of negative segment# 
teg3pout ^ [-12.3,-177.7718]; 36 outside endpoint of negative segment # 

36 

seg4oin - [-36.8686,6.8324]; 36 inside ax^int of positive segment # 
seg4oout •= [-160.2049,78.0606]; 36 outside endpoint rtf'positive segment# 
aeg4pin == [-36.8686, -6.8324]; 36 inside endpoint of negative segment # 
segtpout»[-160.2049, -78.0606]; 36 outside endpoint of negative segment # 

36 

seg3nin == [-12.3,-33.3333]; 36 inside endpoint of positive segment # 
segSnout = [-12.3,177.7718]; 36 outside endpoint of positive segment # 
seg3pin = [-24.3686,28.3030]; 36insideen^iointofnegahvesegment# 
seg3pout - [-147.7049,99.7112]; 36 outside endpoint of negative segment # 

36 

seglnin = [24.3686,28.3030]; 36 inside en^int of positive segnoent# 
seglnout ~ [147.7049,99.7112]; 36 outside endpoint of positive segment # 
seg2pin = [12.3,33.3333]; 36 inside endpoint of negative segment# 
seglpout = [12.3,177.7718]; % outside endpoint of negative segment # 

36 

flag”0; 
while (tri “ 0) 

36testfootl inleg 1 workspace 
11 = 1 ; 

[intept, distl, flag]=arcint(footl, direc, cenbod, outrad, seglnout, seglpout, il); 
if(flag==l) 
dista = distl; 
end 

[intept, distl, fla^=arcint(feotl, direc, cenbod, inrad, seglnin, seglpin, ft); 
if(flag= 1) 
dista = distl; 
end 

rotate >=0; 

[intept, distl, flag]^gint(footl, direc, seglpin, seglpout, rotate); % CCW segment 
if(flag= 1) 
dista = distl; 
end 

rotate => 1; 
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[inicpi. diitl, flag]*wgiiil(footl, dine, seglnin, seglnout, roUteX %CW legmeot 
if(fUg“ 1) 

(tiiU'diAl; 

cad 

K 

H test foots in leg 3 woriopace 
ft = 3; 

[inept, diets, fleg]~ercin(foot3, dine, cen>od, outnd, segSnout, segSpout, ft); 
if (flag •• 1) 

m d|gt3^ 

end 

[inept, distS, flag]-anint(fi>ot3, dine, oen>od, inad, segSnin, segSpin. ftX 
if (flag—1) 
w dtst3« 
end 

route *0; 

[inept, diets, flag]-*cgin(fi>otS, dine, eegSpin, aegSpout, rotate), % CCW ee^nen 
if (flag— 1) 
s dist3^ 
end 

route * 1; 

[inept, dik), flag]>segin(foot3, direc, eegSnin, eegSnout. rotate), %CW segment 
if (flag — 1) 
disib ^ dist3; 
end 
% 

S test foots in leg J woriespaee 
ft-3; 

[inept, diets, flag]>arcint(footS, diree, eenbod, outnd, segSnout, segSpout, ft); 
if (flag — 1) 
a dlSt5^ 
end 

[inept, diets, flag]-arcin(footS, diree, eenbod, innd, segSnin, segSpin, ft), 
if (flag — 1) 
dieted dietS; 
end 

rotate “0; 

[inept, dins, flag]«oegin(foatS, diree, segSpin, segSpout. rotate), % CCW segmen 
if (flag— 1) 

Hiffr B - 

end 

routes 1; 

[inept, distS, flag|=segin(ftiotS, diree, segSnin, segSnout, rotate), %CW segmen 
if(flag— 1) 
diste = distS; 
end 
% 

% fprinf(nstuff,' LI = %S.lf,dista); 

% QtrinfCinstuff,' Lo - %S.lf,distb); 

% ^trinfCnstuff,' LS =%S.lf,diste), 
rnsjedist min( min(dista, distb) diste); 

% 

break; 

end % this ends tripod 0 testing 
% 

while (tii — 2) 

%test foots in leg 2 wotkiqiaee 
ft = 2 ; 

[inept, distS, flag]^arcin(fi>ot2, diree, eenbod, outnd, se^nout, segSpout ft), 
if (flag — 1) 
dista~ distS; 
end 

[inkpt distS, flag]=arcin(foot2, direc, eenbod, inrad, se^nin, segSpin, ft); 
if (flag— 1) 
dista = distS; 
end 
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raOte^O; 

(iaicpi, dikz. iUig]>Mginl(&>al2, dirtc, teglfia, legZpoiit, ratiteX % CCW segmeol 
if (flag—1) 

cod 

rotate = 1; 

[iatG{it,dtlt2,flag]>iegjbal(fi>ot2,direc,ieg2iiiii,aeg2iiout.totateX%CW tegpMitt 
if (flag — 1) 

m dilt2; 

end 

% 

% teet fi>at4 in leg 4 wofiopace 
ft ”4; 

[iitept, di(t4, flag]>arcait(foot4, direc, oenbod, outrad, teg4fiout, seg4)X]ut, ft); 
if (flag — 1) 

^stb~diat4; 

end 

[iatqpt, dist4, fla^*arciiit(foot4, direc, ceabod, torad, aeg4ciiii, seg4piii, ft); 
if (flag — 1) 
distb > dist4; 
end 

rotate “0; 

[iatcpt, diat4, flag|'=segint(foot4, direc, ieg4pin, leg^iout, rotale>, % CCW segment 
if (flag—1) 

(&tb = dist4; 
end 

rotate = 1; 

(iidcpt, diat4, flad-segiiit(foot4, direc, seg4niii, teg4noiit, rotateX CW segment 
if (flag—1) 

(Ustb • dist4; 
end 
% 

%test footd in leg 6 woricspace 
ft “6; 

[intqpt, distd, flag]-arciiit(foot6, direc, cenbod, outrad, segfinout, segfipout, ft); 
if (flag—1) 
distc~distd; 
end 

[intcpt, distfi, fla^=arcint(foat6, direc, cenbod, inrad, segdnin, segdpin, ftX 
if (flag—1) - 
<fistc~dist6; 
end 

rotate >0; 

[intcpt, distd, flag]^segiQl<foot6, direc, segSpin, segSpout, ratateX%CCW segment 
if (flag—1) 
distc > distd; 
end 

rotate" 1; 

[iidcpt, dist6, flagl"segiiit(foot6, direc, segdnin, segdnout, rotate); % CW segment 
if (flag—1) 
distc"dist6; 
end 
% 

% fprintfl[testuff,' L2 = %S.lf,dista); 

% fprintf^testufr,' L4 = %5.1f,distb); 

% ^jrintfl'mstufi’,' L6 = %3.1f,distcX 
maxdist" iiiin( min(dista, distbX distc); 

% 

break; 

end % this ends tripod 1 testing 
% Title: mk-m 

% Purpose: Tests maxdist routines at-)-/-? degree interv als. 

•/,*«•*•••«•••*•******»***•**»*•**•*«••••***»***•••*••**•••**•*•**•••* 

% 

function mkO 
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% 

ftl-C9«.02,0]; 

fi2>[12.S,83]; %{49.01,84.8S78]; 
fl3>H9.01.84.8878]; 
ft4»[-97.0]; S(-98.02,0]; 
ft5*[-49.01.-84.8878]; 
ft6>[12.3,-83]; %(49.01.-84.8878]; 
cenbod~[0,0]; 

% 

!delniitiiff 

% 

1ri*0; 

^iriatfC^iiituff,' Tripod 0 - FOSrnVE\n'); 

. 1 T-n I r. \n\o'); 

dtrw ~ 0:90:360 
^iriiitfCinitiiff.'d » %4.Qf,dinc); 

[m] = md(ftl^;ft3jS4JlSjft6,direc,oenbod,tri); 
m 

4iriidfl>iituff,' iiiax>%3.1fn'^); 
end 
% 

4)riotf(tertuff,*\n Tripod 0 - NEGA'nVE\n')', 

fordir«c = -0:-90:-360 
^jtintfCinstuff.'d - %4.0f,direc); 

[m] > iiid(fll^^^4^4l6,diiec,oenbod,tri); 
m 

^irintf(iiiitiiff,' niax = 34S.lfn'4n); 
end 
% 

tri = 2; 

% 

4>rint<(testuff,‘\n\n\o Tripod 1 • POSlTlVE\n'); 

‘rn.i'.r i .- i ri-Ti. l i. . i . . ~~ ~' \n\n'); 

for dirM ■‘0:1:360 
fptintlCWluff.'d =■ 3M.0f,direc); 

[m] tnd(ftl^4l34l44tS4i6,dd<ec,cenbod,tri); 
m 

iprintf)>ntiiff,' max> %S.lf\n'^); 
etid 
% 

iptintf(teituff,‘W Tripod 1 • NEGA'nVE\n')', 

lpr in itf(testuflF,' • — — ' —•' ' • -—^- \n\n'); 

for direc = -0:-l:-360 
iprintf(testuff,'d - %4.0f,direc); 

[m] - nid(ftl4l24t3^44U4l6.direc,cenbod,tri); 
m 

fpiintfCmstuff,' max = %3.1f\n'^); 
etid 

This MatUb file contains the I -angles 

parameters used in the tnaxdm, | 

segmLm, and arcinLrc functions. -+- +a 

NOTE: This is NOT an executable | 
progranL | Wangles 

+Y 

paratns.n[i; 27APR93; 2Scm foot only 

These parameters were derived using the arciiit2.m function called 
as shown using Leg ONE ii^Nits as an exart^le: 

[x,y,<m = arcint2(25,0;30,0,0,37.5). 

Leg ONE (x, y): workqtace angles defined fixxn (23,0) 
radius inner arc 37.3 cm: 

positive inner segment (36.8686, 6.8324); 30 degrees 
negative inner segment (36.8686, -6.8324); 330 degree 


I 
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ndiiis outer arc 178.2107 cm: 
podtivc outer segment (160.2049, 78.0606>, 30 degrees 
negative outer segment (160.2049, '78.0606); 330 degrees 

LegTWO()ty): workspace angles defined from (12.3,21.6306) 
radius inner arc 37.3 cm; 

positive inner segment (12.3,33 J333); 90 degrees 
negative inner segnieat (24.3686,28.M30); 30 degrees 
radius outer arc 178.2107 cm: 
positive outer segment (12.3,177.7718); 90 degrees 
negative outer se^nent (147.7049,99.7112X 30 degrees 

Leg THREE (x;y): woilcgrace angles defined ftom (-12.3,21.6306) 
radius inner arc 37.3 cm: 

positive irmer segment (-24.3686,28.3030); 130 degrees 
negative inner segnent (-12.3,33 J333); M degrees 
radius outer arc 178.2107 cm: 
positive outer sepnent (-147.7049,99.7112y, 130 deuces 
negative outer segment (-12.3,177.7718); M depees 

Leg FOUR (X, y); wotkqiace angles defined fiora (-23,0) 
radius iiaier arc 37.3 cm: 

positive inier segment (-36.8686,-6.8324); 210 degrees 
negative irmer sepnetU (-36.8686,6.8324); 130 degrees 
radius outer arc 178.2107 cm: 

positive outer segment (-160.2049,-78.0606); 210de9ees 
negative outer segmem (-160.2049,78.0606y, 130 degrees 

Leg FIVE (x, y): workspace angles defined fiom (-12.3,-21.6306) 
ra^us inner arc 37.3 cm: 

positive iimer segmetd (-12.3, -33.3333); 270 degrees 
negative itmer sepnent (-24.3686,-28.3030X 210 degrees 
radius outer arc 178.2107 cm: 
positive outer segment (-12.3,-177.7718); 270 degrees 
negative outer sequent (-147.7049, -99.7112); 210 degrees 

LegSIX(x,y); workspace angles defined fiom (12.3,-21.6306) 
radius inner arc 37.3 cm: 

positive irmer segment (24.3686, -28.3030); 330 degrees 
negative inner^egmetd (12.3,-33.3333); 270 degrees 
radius outer arc 178.2107 cm: 
positive outer segmerd (147.7049,-99.7112X 330 degrees 
negative outer segmeid (12.3,-177.7718); 270 degrees 

This Matlab file ootdains the [-angles 

parameters used in the niaxd3.ni, I 

segint3 jn, aixl arcint3 jn functiom. ---+X 

NOTE: This is NOT an executable I 

program. j+angles 

+Y 

paramsS jn; 26APR93; 43cm foot only 

These parameters were derived using the arcint2.m fiinction called 
as shown using Leg ONE inputs as an example; 

[x,y,<Ul = arcint2(23,0,30,0,0,37.3). 

LegONE(x,y): workspace angles defined fiom (45,0) 
radius outer arc 149.27 cm: 

positive outer segment (139.0446,54.2967); 30 degrees 
negative outer segment (139.0446, -54.2967); 330 degrees 

LegTWO(x,y): workspace angles defined fiom (22.3,38.9711) 
radius outer arc 149.27 cm; 
positive outer segmeid (22.3,147.5645); 90 degrees 
negative outer segmeid (116.3446,93.2678); 30 degrees 




Leg THREE (x, y): workspace anglei defined from (-22.3,38.9711) 
ndius outer arc 149.27 ana: 

poaittve outer segnent (-116.3444,93.2678), 130depcei 
negative outer segment (-22.3,147.3643), M degrees 

LegFOUR(]^y): workspace angles defined from (-43,0) 
radius outer arc 149.27 cm: 

positive outer segment (-139.0446, -34.2967), 210 degrees 
negative outer segment (-139.0446,34.2967) 130 degrees 

LegFIVE(x,y): workspace angles defined from (-22.3,-38.9711) 
radius outer arc 149.2707 cm: 
positive outer segnient (-22.3, -147.3643), 270 depees 
negative outer segment (-116.3446, -93.2678), 210 degrees 

Leg SIX (x, y): workspace angles defined from (22.3, -38.9711) 
radius outer arc 149.27 cm: 

positive outer segnient (116.3446, -93.2678); 330 de^ees 
negative outer segment (22.3,-147.3643), 270 degrees 


% Title: pltfooLm 
% Inputs: 

% Outputs: pltmetplot 

% Purpose; This Matlab program plots the progression ofKNEE2 and the 
% FOOT ofAquaRobot given a known X foot coordinate and the 
% desired leg stride. 

0^*«*.*«*.*«»**«««»*«***««»***.««**..*««**»««««****««***«*****»*i 

% 

clear 

cig 

ck 

idelphjnet 

boldrtf 

yi^obal-70.7107; 

px »input(TtesentfbotX; O'. 

% 

34 NOTE: use next line for direct entry of next x-coordinate, OR 
34 dx = input(Desired foot X; *); 

34 NOTE: use next two lines to enter stepsize fi'om known foot point 
xin = iiq>ut(T)esired stride: *); 
dx - px + xin; 

34 

[elpse, e, elx] = ellipse(px, dx); 

34 

hip=n; 
kneel >°Q; 
knee2 = Q; 

foot«0; 

for n = l:length(elpse) 
elpse(n,2) = elps^n^) + yglobal; 

[t0,tl,t2,t3] ~ invl^l,elpse(n,l),0,elpse(n.2)), 
[Igcljc24l]°kin(t0,tl,t2.t3); 
knee2=[knee2ja(l,l) k2(l,3)]; 

foot=[foot;fl(l.l)ft(U)]; 

end 

hip-Ih(l,l)h(U)]; 
kneel=(kl(l,l)kl(l,3)]; 
axis((-10 180-10060]) 
plot(hip(l,l)4up(l,2V+c2’) 
hold on 

title(EIliptical Foot Motion*) 
xlabelCAquaRobot X-coordinates*) 
ylabelCAipiaRobot Z-coordinates*) 

34 

plol(kneel(l,l)Jcneel(l,2),'+c3') 
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% 

plol(kiiMa(l.l)J(i>M2(UX'*c87 

plQl(kiicc2(p, l)JaMe2(pr2X'^ 
end 

plal(k]ice2(n.lXlciiee2(i42X‘xc8^ 

H 

plo«(fb«H(l.l)/ooi(UX '*«0 

fbtp^Url 

pl^foot(p.l),fi>«<(p>2X'oc6^ 

plot(foat(ii.lX£>ot(n^X‘»^') 

% 

te«(0.3.0.38,'Hip','K0 

teia(0.4.0.S8.'Kr,’ieO 

te3«l(0.2.0.18.’*-st»it','K:’) 

te^O.2,0.1 S.'x - soar.W) 

grid 

meUptt 


% Title: fcsetm 
Inputs: 

HOu^mU: kcftmet plot; ksdau file 

HPufpoee: This MatUbprogrim plots the RESET potture for AqusRobot 
% 

idelks 
.'del kcR-met 
clear 
bold off 

sogle01>0*pi/180; 441egl 

sogle02-60«pi/l^; 
si)gleO3>120*pi/l^: ^ ^ 

aii^e04>180*p^l80; 44 leg4 
siigle03*240*pi/180; 44 legS 
ang|e06-300*pi/180; 44 leg6 
44 

s0=37.3; 

il-20; 

s2=J0; 

a3»l<K); 

44 

sngiel^: 

44 

siigle2F>^.4«pi/180; 
sngle2B~66.4*pi/l 80; 

44 

sngle3F=lS6.4*pi/180; 

siigle3B=-136.4*pi/180; 

44 

T00=[oos(siigle01) •siii(angle01} 0 0;siii(angle01) Got(sngle01) 0 0;0 OlOfiOOl]; 
T01-[oos(snglel) -siii(aoglel) 0 aO;siii(siiglel) c^anglel) 0 0;0 0 10;0 0 0 1]; 
HIP“T0O»T01; 

T12={co«(angle2F) -tiii(siigle2F) 0 al;0 0 10;-iiii(angle2F) ■cos(ang:e2F) 0 0;0 0 0 1]; 
KNEE1«T00*T01*T12; 

T23~[cos(aiigle3F) •«in(angle3F) 0 a2»iii(angle3F) cos(angle3F) 0 0;0 0 1 0;0 0 0 1]; 
KNEE2-TOO»T01»T12*T23; 

T34-(l 0 0 a3;0 I 0 0;0 0 I 0;0 0 0 IJ; 
diuy tci 

FOOT1=TOO*T01*T12*T23*T34; 
diary off 
% 

T00»[cos(angle02) -sin(angle02) 0 0-,sin(aiigle02) cos(angle02) 0 0;0 0 1 0;0 0 0 1); 
TO ’ >[cot(anglel) -siiKangiel) 0 aO;siii(snglel) c^inglel) 0 0;0 0 1 0;0 0 0 1]; 
HIP=T0O*T01; 
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T12-(«><aBgte2F) •«ia(aiigle3F) 0 al;0 0 1 0;-cii<aiigk2F) -c<M(iiigU2F) 0 0;0 0 0 1]; 
KNEEl-T00TX)l*Ti2; 

T23>[ciM(aagle3F) -cu^angleSF) 0 a2;siii(ai«le3F) cai(aii«k3F) QOfiOl 0;0 0 01]; 
ICNEE2-TOOTX»1*T12*T23; 

T34-{10 0 s3;0 1 0 0;0 0 10;0 0 0 1]; 
dUtyks 

FOOT2-TOOnt)l*T12*T23*T34; 
diaiy off 
% 

T00-(cat(aiig|e03) •«m(aogle03) 0 0,siii(aiigle03) c(M<«iigle03) 0 0;0 0 1 0;0 0 0 1]; 
T01-(CM(aiigiel) -*tii(aiiglel) 0 a0,siii(aiigicl) c(a(ai^l) 0 0;0 0 10-,0 0 0 1]; 
HIP-T0O*T01; 

T12~[coi(ingle2B) -<in(aiigle2B) 0 <1;0 0 10;-<iii(aiig|e2B) -ooi(aiig|£2B) 0 0;0 0 0 1]; 
KNEE1=T1)0»T01*T12; 

T23-{oat(aiigle3B) •<iii(tngle3B) 0 a2;siii(augle3B) ooi(aii«le3B) 0 0;0 0 1 0;0 0 0 1]; 
KNEE2«T0O*T01*T12*T23; 

T34-[l 0 0 a3;0 1 0 0;0 0 10;0 0 0 1]; 
diaiy kt 

FOOT3-TOO*T01*T12*T23*T34; 
diaiy off 
% 

T00=[oos(aiigle04) -finCaiigleOd) 0 O^inCangleOd) ca6(aiig|e04) 0 0;0 01 0;0 0 0 1]; 
T01=[ooa(aiiglel) ■siii(aiiglel) 0 a0'4iii(aiig|el) c^ai^el) 0 0;0 0 10;0 0 0 1]; 
HIP-TOO»T01; 

T12=(oos(angle2B) •tin(aa^2B) 0 al;0 01 0;-sin(aiigie2B) -cos(angle2B) 0 0;0 0 0 1]; 
KNEE1=T00*T01*T12; 

T23>'[co«(angle3B) -ain(aiigle3B) 0 a2;siii(aiigle3B) ooa(angle3B) 0 0;0 0 1 0;0 0 0 1]; 
KNEE2=T00»T01»T12*T23; 

T34>(1 0 0 a3;0 1 0 0;0 0 1 0;0 0 0 1]; 
diaiy ks 

FOOT4=TOO»T01»T12»T23*T34; 
diaiy off 
% 

T00=[ooa(aiigle05) -siii(aiigie05) 0 0;siii(aiig]e03) caa(anglcOS) 0 0;0 0 10;0 0 0 1]; 
T01’°(ooa(aiiglel) -aiii(aiigtcl) 0 aO;siii(aiigtel) o^anglel) 0 0;0 0 10;0 0 0 1]; 
HlP-TDO^TOl; 

T12=>(cos(aogle2B) •4in(aiiglc2B) 0 al;0 0 1 0;-nii(aiigle2B) -cos(aiigle2B) 0 0;0 0 0 1]; 
KNEE1=TOO*T01*T12; 

T23=[Gas(aiigle3B) -sin(angle3B) 0 a2;sin(angle3B) CQc(angle3B) 0 0;0 0 1 0;0 0 0 1]; 
KNEE2»TOO*T01»T12*T23; 

T34=tl 0 0 a3;0 1 0 0;0 0 10;0 0 0 1]; 
diary ks 

FCX3T3=T00»T01*T12*T23*T34; 
diaiy off 
% 

T00»[cos(angle06) -sin(angle06) 0 0;siii(aiigle06) cos(aiigle06) 0 0;0 0 10;0 0 0 1]; 

TOl =|cos(anglel) -siii(angk:l) 0 a0',siii(anglel) c^anglel) 0 0;0 0 1 0;0 0 0 1]; 
HIP=T0O*T01; 

T12=(cos(angle2F) -sin(angie2F) 0 al;0 0 1 0;-5iii(angle2F) •cos(angIe2F) 0 0;0 0 0 1]; 
KNEE1=T00*T01*T12; 

T23=[cos(angle3F) -siii(aiigle3F) 0 a2;ain(angle3F) cos(angIe3F) 0 0;0 0 10;0 0 0 1]; 
KNEE2=T00»T01*T12*T23; 

T34-(I 0 0 a3;0 1 00;0 0 1 0;0 00 1]; 
diary ks 

Fo6t 6=T00*T01»T12*T23*T34; 
diarv' off 
% 

x=fFOOTl(l,4) FOOT2(1.4) FOOT3(l,4) FOOT4<l,4) FOOT5(l,4) FOOT6(l,4)]; 

y=[FOOTl(2,4) FOOT2(2,4) F<X)T3(2,4) FCX)T4(2,4) FOOT5(2,4) FOOT6(2,4)]; 

axis([-200 200 -200 200]) 

plol(x,y,'o') 

hold 

x=0; 

y=0; 

plot(x,y,’+g') 

xlabelfX Foot Coordinates (cm)') 
ylabelOY Foot Coordinates (cm)*) 
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ti!lcCAi]uIlobat RESET Poatuic’) 
ffid 

tfejaCFOOrn 

glexi(TOOTr) 

Ctata^ 

meUkcR 

HTtoMATLAB program finifa the mttmfrtimirf the 
% deioed dtracitoa ^travel and the woffapece. 

% 

% Wiittea by Chuck Scfaue. 29DEC92 

HiLm 

% 

% Notes: 1. Aquerobot positive angle/rotstioo is CCW. 
% 2. Matleb positive ai^le/TOtation is CW. 

% 3. Leg One 

% 

clear 

cic 

dg 

FUg-0; 

HDecUntioos 

RX1I-1S7.6S44; 

Ryii>76.6034; 

Rxp-157.6g44; 

Ryp--76.6054; 


% Define line (pas_leg_wo(kspace_liniit) 
tbetapin~>30; 
tbetair>thetapin*pi/180; 
xlinep-23+(12.3*cos(lheUp))-, 
ylinep-12.3 *sin(thetap); 


% Define line (neg;_leg_wofkapace_limit) 

thetaniiF°’30; 

tbetaii>thetanin*pi/l 80; 

xlinea»23-i<12.3*cos(thetan)); 

ylinen-12.3*ain(thetan); 


% Define arc (max workspace limit) with radius 178.2107 
% 

cmix=Q; 

r-178; 

x=(160:0.3:178];%actuaUy 137.6844 to 178.2107 
form=l;lengtli(x) 
y=sqtl(r^-x(:jn)^); 
caux»[cnia]Qx(:mi) y]; 
end 

x=in8:-0.3;160];%actuaUy 157.6844 to 178.2107 
forn=l:length(x) 
y=-iqrt(r^-<: 4 i)^); 
cmax>(cmax;x(.,n) y]; 
end 

% Define arc (inin_wofkq>ace_limit) with radius 37.3 
% 

cfflin^Q; 

r=37.5; 

X-P6.9735:0.1;37.5]; 
forin=l:lenglh(x) 
y=sqit(r^-x(:jn)^); 
aam=‘lcnuB;x(:jn) y]; 
end 

x=p7.5:-0.1:36.9735]; 

forn»l:leag|h(x) 

y=-sqrt(r^.x(:^)^); 









cniiii-(cniiii;x(:4>) yj; 
end 

% Define ny (fbotpoint and deaded ducction of travel) 
tlieUitf*inputCDeeirad_Difectiaa of travel: *); 
if (thetain^-M) 1 (theuin—-270) 
tlietar--89.3*]^180; 
ebeif(thetain«--90) | (thetain>°^70) 
thetaf~89.3*pi/180; 
elM 

thetar-4betain*pi/180; 

end 

xray^inputCEnter knovn footpoint, X coordinale: *); 
yrayoiqHitOEalcr known footpoint, Y coordinate: y, 

% Determine if new footpoint is within poe tc vcg leg limits 
distL|in^tinen-yray)*c^tbetanHxliiMa xray)*sin(thetan); 
distLpp^linep-yray)*oos(thetap)-(xlinep-xray)*tio(thetapX 


if (((distLpipK)) I (distLpn°-0)) A ((distLpp<0) | (distLpp>»0))) 
% Determine wbether there is a positive intenectioo 
thetapos^thetar-thetap; 

if (thetapoe==0) | (theii^>os=^) | (thetapos=-pi) 
disp(l)esired_Diiection and Positive_Leg_Lii^ are parallel.') 
xintpoe-xray; 
yintpos-yray, 
else 

b^xlinep*sin(thetap)Xyiinep*cos(tbetap)); 

a=^xray*sin(thetar)>(jTay*CM(thetar)); 

xto()K'<^thetap)*a)+{cias(thetar)*h); 

ytop><sin(thetar)*b)-(sin(thet^)*a); 

bottam-s^(thetap-<hetar); 

xintpos«'Xtop/bottoni; 

yintpas=ytop/bottom; 

% Determine if the ray intenecis the pos line segment 
Lp>(xiinep*oos(thetap)+ylinep*sin(thetap))', 
Rp>‘(Rxp*cos(tbetap]^Ryp*si[<thetap)); 
segtesr-(xiii4x]s*ccs(thetap>«-yiii4>os*nn(thetap)); 
if ((Lp<segtest) A (Rp>segtest)) % if true, intersection 
% Deteimine'ifthe orientation is correct 
p2>Kxiidpos*oos(thelar)*-yintpos*sin(thetar)); 
pl=(xraj^cos(thetar)+^y*sin(thetar)); 
ifp2>pl % correct orientation 
Flag-1; 
end 
end 
end 


% Determine whether there is a negative intersection 
thetaneg-thelar-thetan; 

if (thetaneg--0) | (thetaneg—pi) | (thetaneg—-pi) 
dispCDesiied_DirectioQ and Negative_Leg_Limit are parallel.') 
xintneg-xTay; 
yintneg-yray, 
else 

b-(xlinen*sin(thetan))-(yiinen*cos(tbetan)); 

a-(xray*siii(thetar)>i^y*eos(thetar)); 

xtop-(-cos(thetan)*a>Kc^th^)*b); 

ytof>-(sin(thetar)'%)-(siii(thetan)*a); 

bottoni-sin(thetaivfiietar); 

xintneg-xtop/bottom; 

yintneg-ytop/bottom; 

% Determine if the ray irrtersects the neg line segment 
Lii=(xlinen*cos(thetan>i-ylinen*$in(thetan)); 
Rn=(Rxn*cos(tbetan)+Ryn*sin(the^)); 
segtat-(xirttneg*cos(thetan>t-yirrtneg*sin(thetan)); 
if ((Ln<seglest) A (Rn>seg'est)) % if true, intersection 
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*/» Ddcnnine if the orientation is conect 
p 2 =(xintneg*cas(thetar>> 7 intDeg:*sin(thetar)); 
p 1 “(xniy*cos(theUr)+yray*$in(thetar)); 
ifp2>pl ^ cofTcct orientation 
Flagyl; 
end 
end 
end 

% Calculate the line segnients 
posint^Q; 

X'^xlinep: 1:160; 
forp=l;length(x) 
y=(-0.5774*x(:,p)>^-14.4338; 
posint={posintpt(;,p) y], 
e^ 

negint^O; 
x^xlinen: 1:160; 
for q=l:lenglh(x) 
y={0.5T7»!c(:,«i)>14.4338; 
neginf=[negint;x(:,q) y]; 
end 

% Draw the orientation of the ray 

ray=D; 

ifFlag=l % positive intersection is good 
if xin4>os<xray 
x=xintpos: 1 :xray; 
else 

x=xray: 1 :xintpos; 
end 

m=(yray-yin^>osy(xray-xintpos); 

b=yray-{m*xray); 

elsetf Flag==^l % negative intersection is good 
if xintneg<xray 
x=xintneg;l:Kray; 
else 

x=xray:l:xintneg; 

end 

ni=(yintneg-yray)/(xintneg-xray); 

b=yTay-(m*xray); 

else 

Flag=0; % no intersection with line segments 


% -M-(-M-++iiisert intersection with arcs here+++ 


end 

for g=l:length(x) 
y“{m*x(:,g))+b; 
ray=(ray,x(:,g) y]; 
end 

% Plot the results 
axis([0 200 -100 100]) 
axisCsquare') 

plot(posint(:, l),posint(:,2),'g) 
hold on 

plot(negint(:, l),negint(:^),'g;) 
plot(cmin(:,l),cmin(:,2),'^ 
plol(cmax(:,l),cmax(:^),’gf) 
grid 

plot(xray,yTay,'+c5') 

ifFlag==l 

plot(xintpos,yintpos,'or^ 

elseifFlag=-l 
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plot(xinlDeg,yiotaeg,'otO 

else 

FUg-0; 

text(0.25,0.2,'Ray ietenects only srcs.'.'sc') 
end 

plo«(r«y(;,l)^y(:A'*>') 

titleCInlenectioQ of Directioa ind Wotkspace') 
xUbel(?< coordinates, (cm)') 
ylabe](T coordinates, (cm)') 
bold off 

else % new fiootpoiot not inside pos&neg leg limits 
% Calculate the line se^nents 
posint=Q; 
x=xlinep:l:160; 
farp-l:length(x) 
y-<-0.5774»x(:4)))+14.4338; 
posint-[posiiit^: j>) y]; 
end 

negint-Q; 
x>xliaen: 1:160; 
for q=l:lenglli(x) 
y=(0.577*x(:,q)>14.4338; 
negint=-(negiiit;x(:,q) y]; 
end 

% - T^- III. - 

% Plot the results 
axis([0 200 -100 100]) 

plot(posiiit(;,l),posiiit(:,2),'g') 
bold on 

plot(oeginl(:,l)jiegint(;,2),'g) 

plot(cmin(:,lXcmin(:,2X'g') 

plot(cmax(:,lXcmax(:,2X'g') 

grid 

plot(xriy,yTiy,'+c5') 

title(lnteisection of Direction and Workspace*) 
xlabel(7C coordinates, (cm)*) 
yiabel(T coordinates, (cm)5 
text(0.2S,0.2,Tootpoint not inside iiinits.','sc') 
hold off 
end 


% Title: segintm 
%Iiq)uts: (1,2,3,4,5) 

%Oiiqxits: [1,2,3] 

% Purpose: This Matlab function determines the intersection of a ray 
% and a line segment 

% 

function [intcptdistjlag] = segint(foot direc, inseg outseg, vector) 

% 

% intcpt = (x,y) coordinates of intercept 

% dist = distance between foot and intercepts 

%flag= 1 ifintercept found; flag = 0ifno intercept found 

% foot = (x,y) foot components 

%direc = dhectirm of interest 

% inseg/outseg = endpoints of segment 

% vector = 1, line segment is clockwise 

% vector = 0, line segment is counter-clockwise 

% 

limit = 215; % maximum possible distance 
flag = 0; 

direc = direc*pi/180; 

% 

% Find theta of segment 

thetaseg = atan2(outseg(l,2) - inseg(l,2), outseg(l,l) - inseg(l,l)); 
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% 

% Oetenniiie if foot &Us on the line tegomit 

distLp «(((faot(l^>imeg(l^))*co«(tbeUse«)} - ((foo<(l.l>-in*eg(l.l))**«n(tl>e***eg))); 

diitL|> - rnuniKHirtF fX 

if(diittp“0) 

flag = 1; % fiiot is oo tine segment; quit 

Hfd s limit; 

intcpt^ [foot(l.l)4bot(U)]; 
elseif ((i&tL{> > 0) & (vector = 0) ) 
flag > >1; %foot is outside wotlcspace; quit 
dist-linut; 
intcpt^Q; 

elseif ((distLp < 0) & (vector •= 0) ) 
flag = 0; foot is inside workspace; find i n te rs ection 
elseif ((distLp < 0) & (vector “ 1) ) 
flag = -1; %foot is outside workspace; quit 
dist = Iiinit; 
intcpt° Q; 

elseif ((distLp > 0) & (vector = 1) ) 
flag ° 0; % foot is inside workspace; find intersection 
end 
% 

if (flag ”= 0) 
tbetadif = direc - thetaseg; 

if (theUutif 0) & (Ihetadif— pi) & (Iheladif •-= -pi) 

b = (inseg(l,l)*sin(thetaseg)) - (inaeg(l,2)*cos(thdaseg)); 
a = (foot(l,l)*sin(direc)) - (foot(l,2)»cos(direc)); 
xtop - (•cos(thetaseg)*a) + (coe(direc)*b); 
ytop - (sin(direc)*b) - (sin(thetaseg)*a); 
bottom >= si^tbetaseg - dittt); 
xint = xtop/bottom; 
yint = ytop/bottom; 
iotcpt = (xint, yint]; 

% Oetennine if the ray intersects the line segment 

CX:W = (inseg(l,l)*c^tbetaseg) + inseg(14)*sin(*l'*fos®g)); 

CW ” (outseg(l,l)*cas(thetaseg) + oiitseg(l,2)*siii(thetase^); 
segtest = (xint*coa(thetaseg) + yint*sin(thetaseg)); 
if ((CCW <« segtest) & (CW >= segtest)) % if true, intersection 
% Determine if the orientation is correct 
p2 = (xint*cos(diiec) + yiin*sin(ditec)); 
pi -= (foot(I,l)»eos(ditec) + foot(l,2)*sin(direc)); 
ifp2>pl % correct orientation 
flag = 1; 

dist = sqn((yint - foot(l,2))^ + (xint - foot(l,l))^X 
else 

flag = 0; 
end 
end 
end 
end 
% 

if( (flag =-1)1 (flag = 0)) 
flag = 0; 
dist limit; 
intcpt = D; 
end 

% Title: seginlLm 
% Inputs; (1,23,4) 

% Outputs: [1,2,3] 

%PuTpose: This Matlabfiinction determines the intersecticm of a ray 
% and a line segment 

% 

fimction [intcptdist 41 agl = segintl(foot direc, inseg. outseg) 

% 
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% intqpl • (x,y) coorduiaic* of iolcrcqpt 
% diit diflam bc^wccn foot and mIerctpU 
%flag- 1 if ndcfcc^ found; flag-0 if no intcrcqit found 
%foot - (x,y)foat compoaenU 
% dirac ~ directioa of intcrcA 
% inae^outaeg - endpointa of legnicnt 
% 

flag“0; 

direc ~ direc*pi/180; 

% 

% Find thcla of acgpacoi 

thetaseg - atan2(outseg(l,2) - iiifeg(l,2), outugfl.l) - inseg(l,l)) 
% 

% Determine if the foot is on the line legmcol 

CCW - (inseg(l,l)*co*(t>>e(>*eg) + inaeg(l^)*iiii(lheta*eg)) 

CW ■= (outseg(l,l)*cae(tlietaacg) + outaeg(l,2)*aiii(lhetaseg)) 
Kglert - (foot(l.l)*oof(thetaaeg) + foot(l,2)*iiii(thetaieg)) 
piusc 

if ((CCW <= legleft) ft (CW >- aegtest)) % if hue, foot is on line 
flag - 1; 
dist = 0; 
inicpt-foot; 
end 

if (flag -= 0) 
thetadif - direc • thetaseg; 

if (thetadif -=> 0) ft (thetadifpi) ft (thetadif-- -pi) 
b = (mseg(l,l)»sin(lhetaaeg)) - (inseg(l,2)*cos(thetaseg)); 
a = (foot(l,l)*sin(diree)) - (fo<n(l,2)*coe(direc)); 
xtop - (-c»e(thetaseg)*a) + (cos(direc)*b); 
ytop - (sin(direc)*b) • (si^thctafeg)*a); 
boaom - sin(thetaseg • direc); 
xint - xiop/boltom; 
yint - ytop/bottom; 
intcpt - [xint, yint]; 

a Oetennine if the rey intersects the line segment 
CCW - (inseg(l,l)*cos(thetaseg) + inseg(l,2)*sin(thetaseg)); 
CW = (oulseg(l,l)*eos(thetaseg) + outseg(l,2)*$in(thetaseg)); 
segtest = (xint*coi(thetaseg) + yint»sin(theuseg)); 
if ((CCW <» segtest) ft (CW >= segtest)) % if hue, intersection 
% Detennine-if the orientation is correct 
p2 - (xint*oos(direc) + yint*sin(direc)); 
pi - (foot(l,l)*cos(direc) + fo<^l,2)*sin(direc)); 
ifp2>pl correct orientation 
flag - 1; 

dist - sqit((yiiit - foot(l,2))^ + (idiit - foot(l,l))^); 
else 

flag-0; 

end 

end 

end 

end 

if (flag — 1) 
flag - 0; 
dist - []; 
intcpt” [); 
end 


% Title: stabintm 
•/.Inputs: (1,23.4,5.6.7.8,9,10) 

•/•Outpub: [13.3,4] 

•/« Purpose: This Matlab fonction determines the intersection of a ray 
•/. and a line segment in one Tripod. 

•/,•••»•••••••*•*•••••••**•••••••••••••••••••••••••••••*•••••' 

% 

fonction [xint,yint,disUlag] - sUbint(Tjx,ry,rtheta,ax,ay.bx,by,cx,cy) 

•/. 


155 








ST' tripod number, 0 or 1 

S ax/y. bx/y, cx/y ' wlccted tripod foot pocitiaai 

% rx/^ ' ray convonentt 

S rih^ ' direction of inlercat 

S xint/yint» segment intercepis 

% diet'dirtance between ix/ry and xint/yint 

Sflag' 1 ifinteroeptfoum^ flag'Oifnointeroept found 

% lega ' legl/2, lej^ ' leg3/4, lege = legS/6 

% 

S convert degrees to radians and 

% oonect for differences in positive angle oonvenlion used 

% in computer and used on AquaRobot; AquaRobot positive 

SangleisCW 

itheta ' •(itheta*pi/180); 

% 

% correct that -t-y axis in Matlab is -y axis in AquaRobot 

ty«-ry; 

ay = -ay. 

by--by, 

ey = -cy. 

% 

llag'O; 

ifT='0 

%-r- ... , 

% test segment 1-3 (lega-legc) 

% 

S Find theta of segment 
thetaseg-atan2(ay-cy,ax-cx); 

S 

tfaetadif-rtfaeta-thetases 
if (thetadif“H)) | (1hetadif=pi) ] (thetadifo=-pi) 
dispOparaller) S desired direction and line segment are parallel 
else 

b'(cx*sin(thetaseg))-<<=y*cos(thetaseg}); 

a'(rx*sin<ttbeta))-(ty*oot(rtheta)); 

xtop=(-coa(thetaseg)*a)+<cos(itheta)*b); 

ytop-<sin(rtheta)*bKtin(thetaseg)*a); 

bottoi»-sin(thetaseg4theta); 

xlSint-xtop/bottom; 

ylSint-ytop/bottom; 

S Determine if the ray intersects the line segment 
legcpt'(cx*oos(thetaseg>+cy*sin(thetaseg))', 
leg^it-<ax*cos(thetaseg)+ay*sin(thetaseg)); 
seglest=<xl3int*oos(thetaseg)+yl3int*sin(thetaseg)); 
if ((legcpt<-segtest) & (segtest<=legapt)) % if true, intersection 
% Determine if the orientation is correct 
p2=<xl5int*cos(tlheta)+yl3int*sin(rtheta)); 
pi =(rx*cos(rth^>Hy*sin(ttheta)); 
ifp2>pl S correct orientation 
dist=s<pt((y 13int-ryy^+(xl 5int-rxy'2); 
xint - xlSint; 

yint'-yl Sint; % recorrect y axis 
end 
end 
end 

--r-,r 

% test segment S-3 (legc-legb) 

% 

% Find theta of segment 
thetaseg=atan2(cy-by,cx-bx); 

% 

thetadi^'Ttheta-thetaseg; 
if (thetadif='0) | (thetadifo=pi) | (thetadif='-pi) 
dispCparaliel') % desired direction and line segment are parallel 
else 

b'(bx*sin(thetaseg)Hby*cos(thetaseg)); 

a=(rx*sin(ttheta))-{ry*c»s(tthela)); 
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xlap=(-oa«(theUseg)*s>Kcc«(itbeU)*b); 

ytop-(iiii(rtfaeUi)*b>< «in ( <h^ » « * » )*>X 

botioin-iai(tfactaseg4theUX 

xS3iot>*xlo|Vboaam; 

y33int-ytop/boaatn; 

% Daterniiiie ifthe fay inienecli the line tcgmott 
legc|)t°Kcx*coe(thelaseg>H 7 *iin(theUseg)); 
legb|it*^x*cae(tbeUseg>+t^r*siii(thetaseg)X 
tegfBit-(x33iiit*oot(tfaeUieg>+y53int*sin(thetaieg)); 
if ((legcpP>-ieg|eA) A (s^teA>>leg{b|it)) 34 if true, intersection 
34 Determine if the orientation it cotiecS 
p2-(x53int*ooe(rtheta>^3iit*sin(ttheta)); 
pl“(tx*coe(rlh^)+ty^iin(rtheta)X 
ifp2^1 34 oonect orientation 
dut-aqrt((y53mt-fy)^+(*53int-rx)^X 
xint*x33int; 

yint-y33int; % recorrect y axis 
end 
end 
end 

34 test segment 3-1 (lesb-lcgt^) 

% 

34 Find theta of segment 
thetase^ataii2(ay-by,ax-bx); 

34 

thetadifntheta-thetaseg; 
if (thetadif“0) 1 (thetadif==pi) 1 (thetadif=^pi) 
diqtOparaUel') 34 desired direction and line segment are parallel 
else 

b=(bx*sin(thetaseg))-(by*CQs(thetaseg)); 

a=(rx*rin(rtheta)><>y*co<(i1hina)); 

xtcf>~(-co^thetaseg)*a>+<cas(ithM)*b); 

ytop*^sin(itheu)'bH*>t>(t°<'‘**‘S)**)l 

bottoin~ain(thetaseg4thetaX 
x3 lint^xtop/bottom; 
y3 lint^-ytop/bottom; 

34 Determine if the ray intersects the line segment 
legapt=(ax*oos<thetaseg)+ay*sin(thetaseg)); 
legbpt>(bx*oos(thetaseg>'-l^'*sio(thetaseg)); 
segtest=(x31int*oos(thetaseg>+y31itd*sin(tlietaseg)); 
if ((legbp«-segtett) & (segtest<=legapt)) 34 if true, intersection 
34 Determine if the orientation is correct 
p2=(x31 iiit*oos(tthetaX*-y31 int*sin(rtheta)X 
pl=(rx*cos(rtheta>+Ty*sbi(rtheta)); 
ifp2>pl 34 correct orientation 
dist=sqrt((y31ittt-ry)^2+<x31int-rxy^); 
xint = x31int; 

yint = •y31int; 34 reoonect y axis 
end 
end 
end 
% 

elseifT=l 

34 test segment 2-6 (lega>legc) 

34 

% Find theta of segment 
thetaseg=atan2(ay<y,ax-cx); 

% 

thetadi^Ttheta-thetaseg; 
if (tlietadif=0) | (tlietadil==pi) | (thetadif=-pi) 
dispOtarallel') 34 desired direction and line segment are parallel 
else 

b^cx*sin(thetassg))-(cy*cos(lh«taseg)); 

a=(rx*sin(rtheta)Xty*cos(rtbeta)); 

xt<^>=(-<»s(thetaseg)*a)+<cos(rtlieta)*b); 






ytop-(iai(itlica)*bHnn(<l>«>MX)**); 

bottam-niCItNluegitlieU); 

x26iaMitop/boltaiii; 

y26ait-ytop/boaoin; 

H Ddenniiie if the fay imcrsecti the line segment 
lega|iHax*Mi(lhel«Mg>vay*siii(1hnaseg)y. 
legc^t-(cx*oat(lhetascgyK:y*siii(tlieSaa^); 
segtertF^)d6int*ooe(thetas^>t-y26int*siii(thetaseg))', 
if((legcpt<-teglcet) * ($egl«<=leg*|)t)) % if true, intersection 
% Detencine ifthe orienUtiaa it conect 
p2=(x36int*cot(itheta>t-y26int*sin(ftheU)); 
pl^nc*cae(itheU)+iy*tin(itheta)); 
ifp2^1 % conect orientatioa 
dut=virt((^6at-ryy^-^(x26int-rxy^); 
xint x26int; 

yint»-y26iiit; % leoonect y axis 
eod 
end 
end 
H 

%test se^nent 6-4 (legc-le^) 

% 

% Find theta of segment 
1heUseg=atan2(cy-by,cx-bx)', 

% 

thetadifftbeu-thetase^ 
if (tbetadif=0) | (theUdif=pi) | (thetadit"<^i) 
disp(^>artUer) % desired dire^on and line segment are parallel 
elae 

b-(bx*siii(thetaae^><by*cos(tbetaaeg)); 

a-(rx»sio(ffheta)Kfy*«>a(rthet»))‘. 

xtop>^-oo^tfaeta^*a^cot(ttlina)*b); 

ytoi>=(sin(itheU)^><*i“(‘**«‘***g)*»); 

bottoim°sin(thetaseg-itheta); 

x64iat-]^op/bottoaii 

y64inr*yto|i/bottoni; 

% Detennine if the ray intersects the line se^nent 
legcpt><cx*cos(tbetaseg>+cy*$in(thetaseg)); 
leg|bpt-<bx*oos(thetaseg>rtiy*sin(thetas^)-, 
segtest=(x64int*cos(thetase^y64int*sin(thetaseg)); 
if ((legept<=segtest) & (segtest<=legibpC)) % if true, intersection 
% Determine ifthe orientation is conect 
p2“(x64int*cos(rtheU>+y64iijt*sin(rtheta)); 
pl“(t’t*®os(rti>^)*ty**“*(rtif***)); 
ifp2^>l % conect orientation 
dist=sqrt((y64int-ryy^+(x64iiit-rxy^); 
xint = x64iiit; 

yint = -y64in^ % lecorrect y axis 
end 
end 
end 
% 

% - - 

% test segment 4-2 (legh-lega) 

% 

% Find theta of segment 
thetaseg=atan2(by-8y,bx-ax); 

% 

thetadifntheta-thetase^ 
if (thetadif=0) | (thetadif=pi) | (thetadif=-pi) 
dispCparaller) % desired diiection and line segment are parallel 
else 

b={ax*sin(thetaseg))-(ay*eos(ii*etaseg)); 

a=(rx*sin(itheta))-(ry*cos(ilheta)); 

xtop=(-c<^theta^)*a)+(cos(rth^)*b); 

ytop=(sin(rtheta)*b>-(s®(lhetaseg)*a); 









batlaimin(thetaa^<tbeu); 

x42iflt-xlop/boaom: 

y42iDt*7lo|)/bo(taai; 

% Dttamiiie iftfae imy m t a urti tht line ngnent 
le8*|it<K*x*G0i(tfaelaie^^iiy*sii>(tfaclaicg)); 
kgbpt-^x*c<n(tfatU»eg) I t^ Hn ( thc« ai i c t }X 
seglc)t-<x42iiit*cai(tl>ctaseg>4^2iat*<in(theUseg)>, 
if ((kgi|)t<-ie8lat) ft (scgtcsK-Icgijpt)) %iftnie,iiiunecuaii 
% Ddenniiie if the OficnUtioa is ootrea 
p2^x42int*cas(itheU>'742int*siii(ftheU)); 
pl>(ix*oai(ithctt>tTy*siii(itl>eU)); 
ifp2>pl % ooRcct orienUtiao 

dist=4qrt((y42iiil-ryy^+(x42inl-ncy^X 
xint - x42int 

yint >-y42inl; % leccmct y axis 

Old 

end 

end 

% 

else 

disp(lncortect Tripod number^ 
end 

ifdist=“n 

flag-0; 

else 

flag-1; 

end 

^.«.«.••••«*«.*.*«**«*•.«*•.***•*«•«*••••**•••***•••*•*••••*•* 

% Title: stable jn 
%Ii¥Uts: (U3,4,5.6,7,8) 

% Outputs: [1^] 

%Puipase: This Matlab function calculates the stability of a Tripod. 

% 

fiinclioo [fla&tm]-stable(CBx,CBy.legax,legay,legbx,legby,legcx,legcy) 
•A CBJe^legb,legc ocienled CCW so S is positive when CB is inside. 

% 

*A Hag’ indicates degee of stability 
%'em'is the stability margin 
% iega-iegl/2, legl^leg3/4, legc-iegS/6 
xl - CBx; 

yl - -CBy, %t-y axis in Matlab is -y axis in AquaRobot 
x2-legax; 

y2 = 'legay; %+y axis in Matlab is -y axis in AquaRobot 
x3-tegbx; 

y3 - -legby, %+y axis in Matlab is -y axis in AquaRobot 
x4 = legcx; 

y4 = -legey; %+y axis in Matlab is -y axis in AquaRobot 
% 

U = S<pt((x3-x2r2+(y3-y2r2); 

U = S<pt((x4-x3r2+(y4-y3r2); 

LI = S<pt((x2-x4r2+(y2-y4r2); 

% 

51 = ((x2-xl)*(y4-yl)-(x4-xl)*(y2-yl)y2; 

52 = ((x4-xl)»(y3-yl><x3-xl)*(y4-yl)y2; 

53 = ((x3-xl)*(y2-ylHx2-xl)*(y3-yl)>2; 

% 

if (S1>0) ft (S2>0) ft (S3>0) 
flag-1; % The new tripod is stable, 
elseif (S1<0) | (S2<0) | (S3<0) 
flag--l; % The new tripod is unstable, 
else 

flag-0; % The new tripod is nuuginally stable 
end 
% 

H1-2»S1/L1; 
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H2-2*S2/U; 

H3-2*S3/U; 

% 

atf’naiaSHimmjy. 

K Title: sUrtin 

^Outputs: kcS-met plot; ks data file 

% Puipote: This Matlab program piols the START pasture for A q uaRobot 

^ »**»*«»»•••*«*•••••*»*•••••••••••••••*•••••••••••••••••••••»••»•••••• 

% 

Idelks 

IdeikcSjiiet 

clear 

boMoff 

% 

aiigle01^*pi/180; Hlegl 
aii^e02=60*pi/180; %leg2 
angle03>120*pi/180; % leg3 
aiigle04=I80*pi/180; % leg4 
aiigle0S>240*pi/180; % leg5 
aiigle06=300*pi/180; % leg6 
% 

a0»37.3; 

al^O; 

a2=50: 

a3«100; 

% 

angle! > 0; 

% 

angle^F = .33.8383*pi/180; 
angle2B = 35.8383»pi/180; 

% 

angle3P * I25.8383*pi/I80; 
angIe3B «•123.8S8S*pi/180; 

% 

TOO ^ (cos(angle01) •<in(angie01) 0 0;sin(ingle01) cos(aagle01) 0 0;0 0 1 0;0 0 0 !]; 

TO! [cosfanglel) -sinfanglel) 0 aO;sin(angiel) o^ai^el) 0 0;0 0 1 0:0 0 0 !]; 

HIP = T0O*T01; 

T12 = (cos(angle2F) •siii(angle2F) 0 al;0 0 1 0;-siii(ingle2F) -cos(angIe2F) 0 0;0 0 0 !]; 
KNEE! = TOO»TO!»T12; 

T23 = {cc<(angle3F) -siii(angle3F) 0 a2'^iii(angle3F) co((aDg!e3F) 0 0;0 0 ! 0;0 0 0 !]; 
KNEE2 = T00»T0!*T!2*T23; 

T34 = [! 0 0 a3;0 ! 0 0;0 0 ! 0;0 0 0 !]; 
diary ks 

FOOT! =TOO»TO!*T!2»T23»T34; 
diary off 
% 

TOO = {cas(angle02) •sm(angie02) 0 0'4in(angle02) cos(angle02)0 0;0 0 10;0 0 0 !]; 

TO! = [cos(angIe!) -sio(angIe!) 0 aO;sin(angle!) c^angle!) 0 0;0 0 ! 0:0 0 0 !]: 
HIP-T00»T0!; 

T!2 = [cos(angIe2F) -shi(angle2F) 0 a!:0 0 1 0;-siii(angIe2F) -cos(angle2F) 0 0,0 0 0 !]; 
KNEE! =TOO*TO!*T!2: 

T23 = [cos(angle3F) -stii(angle3F) 0 a2:siii(angle3F) cos(angle3F) 0 0;0 0 ! 0;0 0 0 !]; 
KNEE2 = TOO»TO!*T!2*T23; 

T34 -= (10 0 a3;0 ! 0 0;0 0 10,0 0 0 !]; 
diary ks 

FOOT2 = TOO‘TO!*T!2*T23»T34; 
diary 
% 

TOO = [cos(angle03) ■siii(aiigle03) 0 0:sin(angle03) cos(angle03) 0 0;0 0 ! 0:0 0 0 !J; 

TO! = [cos(anglel) -sin(angle!) 0 aO:sin(ai]gle!) c^angle!) 0 0:0 0 ! 0;0 0 0!]; 

HIP = TOO*TO!; 

T!2 = [cos(angle2B) -5in(aiigle2B) 0 a!:0 0 ! 0:-5in(aiigle2B) -C03(angle2B) 0 0;0 0 0!J; 
KNEE! =TOO‘TO!»T!2; 

T23 = [cos(angle3B) -sin(angle3B) 0 a2:sin(angIe3B) cos(angIe3B) 0 0;0 0 ! 0;0 0 0 !]; 
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KNEE2 - T00*TD1»T12‘T23; 

T34 - [1 0 0 a3;0 1 0 0;0 0 1 (HO 0 0 1]; 
diary ks 

foots - T00*T01*T12*T23*T34; 
diary 
% 

TOO - [oatCaiigMM) -fm(aiiglc04) 0 0-,iin(aiig|c04) oaa(angle04) 0 0;0 0 10;0 0 0 1]; 

TOl • [coi(aiiglcl) •«ii(aiiglel) 0 aO',iin(angicl) c^aqglel) 0 0;0 0 1 (HO 0 0 1]; 
HIP-T0O*T01; 

T12 > [ooa(aiigle2B) -fiii(aiigle2B) 0 al;0 0 1 0:-tiB(tagle2B) •cat(aiigie2B} 0 0;0 0 0 1]; 
KNEEl - T00*T01*T12; 

T23 > [oof(aiigl^B) -tiii(aiigle3B) 0 a2;sin(aogle3B) coa(a n g|fcSB) 0 0;0 0 1 0;0 0 01]; 
KNEE2 - T0O»T01*T12*T23; 

T34 - [1 0 0 a3;0 10 0;0 0 1 0;0 0 0 1]; 
diary ks 

FOOT4 - T0O*T01»T12»T23»T34; 
diary off 
% 

TOO > [oot(angle0S) -siii(aiigie05) 0 0,sin(aiislc03) oos(aiig|e03) 0 0;0 0 10;0 0 0 1]; 

TOl = [cosCanglel) -tiii(anglel) 0 a0;sii)(aiiglel) c^anglel) 0 0;0 0 10;0 0 0 1]; 
HIP-T0O*T01; 

112=° [coi(angte2B) -siii(afig|e2B) 0 al;0 0 1 0;-siii(angle2B) -cot(angIe2B) 0 0;0 0 0 1], 
KNEEl “T0O»T01»T12; 

T23 ~ [oos(angle3B) -ainCanglcSB) 0 a2.sin(aiigle3B) cos(ang)e3B) 0 0;0 0 1 0;0 0 0 1]; 
KNEE2 - TOO»T01*T12‘T23; 

T34 - (10 0 a3;0 1 0 0;0 0 1 0:0 0 0 1]; 
diaiyks 

FOOTS - T0O*T01»T12‘T23*T34; 
diary off 
% 

TOO = (caa(aiigteOd) -sinCangleOd) 0 0'4iii(ang|c06) cos(afigle06) 0 0;0 0 1 0;0 001]; 
TOl > [oos(aiiglel) -tifi(anglel)0 aO,siii(aiigtel) c^anglel)0 0;0 0 10;0 0 0 1]; 
HIP-TOO'TOl; 

T12 - (oos(aiig|e2F) -dt<angle2F) 0 al;0 0 1 0;-tii>(aiigle2F) -oot(angle2F) 0 0;0 0 0 1]; 
KNEEl-TOO»T01»T12; 

T23 “ [cos(angie3F) -aii^angleSF) 0 a2;sin(angle3F) ooi(aiigle3F) 0 0;0 0 1 0;0 0 0 1]; 
KNEE2 - TOO»T01»T12*T23; 

T34 - [1 0 0 a3;0 1 0 0;0 0 1 0;0 0 0 1]; 
diary ks 

FOOT6 - T00*T01»T12»T23*T34; 
diary off 
% 

X - [FOOTl(l,4) F(X)T2(1,4) FOOT3(l,4) FOOT4(l.4) FCX)T5(1,4) FOOT6(l,4)]; 

y - [FOOTl(2,4) FOOT2(2.4) FOOT3(2.4) F(X)T4(2,4) FOOT5(2,4) FOOT6(2.4)]; 

axis([-200 200-200 200]) 

plot(x,y,'o’) 

hold 

x«0; 

y-0; 

plo«(x,y,’+g^ 

xlabel(^ Foot Coordinates (cm)') 
ylabel^ Foot Coordinates (cm)T 
titleCAquaRobot START Posture') 
grid 

gtextOFOOTn 

gtexl^OOT2^ 

glexlCBodyCaitetO 

roeUkcS 


Si Title; ucwlinkl.m 
Si Inputs: 

Si Outputs; c4.metplot 

Si Purpose; This Mstlab program plots the AVAILABLE woric^jace of 
Si the HIP (Joint 1) to KNEEl (Joint 2) LINK 1 of the AquaRobot. 

Si 
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!deio4jiiet 

inglcO-OVlSO; 

angk 2 - 0 *i>i/ 180 ; 

aO-37.5; 

al-20; 

% 

»lvec“|]; 

fac logleW-MVlSOyO Vl>0);(60*pi/180) 

7BO*[ooi(aBgleO) -«in(angl«0) 0 0; 
siii(aa{jcO) cai(aiigleO) 0 0; 

0010 ; 

0001 ]; 

% 

T01>[ca6(aiiglel) -<iii(aiiglel) 0 aO; 
aiii(aiiglel) caa(anglel) 0 0; 

0010 ; 

0001 ]; 

H 

T12>[coi(aii^e2) -siii(aiig|e2) 0 al; 

00 10 ; 

-aiii(an^e2) -ca<(ang)e2) 0 0; 

00 01]; 

% 

UNK1=TB0*T01*T12; 
alvec«[alvec;LINKl(l,4) UNK1(2.4)]; 
end 

ax>s((0 60-30 30]) 

plo((alvec(:.lX>lve<^:.2)) 

grid 

title(71ip Workspace*) 
xlabelCX coordinate, cm*) 
ylabel(T coordinate, cm^ 
gtexlCHIP') 
gtextC-60 degrees') 
glextCOO degrees') 
gtext(lCN£El') 
metao4 

•/,*********««««*««***«*«**«««***M**»«***«*****«««««*******«mM*i 

% Title: uc\vitnk2jn 
% Inputs: 

%Oinpuls: cSjnetplot 

% Purpose: This MtUlab propam plots the AVAILABLE workspace of 
% the KNEEl (Joint 2) to KNEE2 (Joint 3) LINK2 of the AtiuaRobot 

% 

!del c3.inet 
% 

angIeO^*pi/lSO; 

anglel=0*pi/180; 

angle3»0*pi/l*0; 

a0=37.5; 

al=20; 

a2=50; 

% 

a2vec-Q; 

forangle2=(-73.4»pi/l*0):(5»pi/180):(106.6*pi/180) 

TBO=[cDs(an^eO) ^^(angleO) 0 0; 
sio(angleO) cos(angleO) 0 0; 

0010; 

0001 ]; 

% 

T01>[cos(anglel) -sin(anglel) 0 aO; 
sin(anglel) cos(anglel) 0 0; 

0 010; 

0001 ]; 

% 
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T12-[ca«(aiigte2) -«iii(uig|e2) 0 al; 

0 010; 

-8iii(angle2) -oat(angle2) 0 0; 

0001 ]; 

% 

T23*[eo((aiigle3) -cin(aiigle3) 0 a2; 
tin(aiigk3) ooi(angl^) 0 0; 

0010; 

0001 ]; 

% 

LINK2=TB0»T01*T12*T23; 
a2vec-‘[a2vec-XINK2(l,4) LINK2(3.4)]; 
cod 

axi^EOnO-dOM]) 

P>ol(a2vec(:,lXa2v»c(:^)) 

grid 

tiUcCKneel Wockipace') 
xlabelCX coordmate, cal') 
ylabeIOZ coordinate, cal') 
glextCKNEEl') 
glext^.4 degrees') 
g|textC-106.^ degees') 
glext(1CNEE2') 
metacS 


% Title: ucwliiik3.ni 
%Ii^Hits: 

%OvRputs: o6jnetpIot 

% Purpose: This Matlab program plots the AVAILABLE workspace of 
% theKNEE2(Joint3)toFOOT(Joiiit4)LINK3oftheAquaRot>oL 

V. **•*••••«••*•*•*•*•••****•*•••«•**•*••••••••••*•*•••••••*»**•* 

% 

!del c6.niet 
% 

anglcOK)*pi/180; 

anglel=0*pi/180; 

angle2=O*pi/180; 

aO=37.S; 

al=20; 

a2=50; 

a3-100; 

a3vec=Q; 

foraiigIe3=(-23.6*pi/180):(5»pi/180):(156-4*P'/180) 

TB0=[cas(an^e0) -$iii(angleO) 0 0; 
sin(sngleO) cos(angleO) 0 0; 

0 010; 

0001 ]; 

% 

T01='[cos(anglel) -sin(anglel) 0 aO; 
siii(angiel) co^anglel) 0 0; 

00 10 ; 

0001 ]; 

% 

T12=[cos(angIe2) -sin(angle2) 0 al; 

0010 ; 

-sin(sngle2) -cos(angle2) 0 0; 

0001 ]; 

% 

T23=[cos(angle3) -sin(aiigle3) 0 a2; 
sin(aiigle3) cos(angIe3) 0 0; 

0010; 

0001 ]; 

% 

T34«[l 0 0 a3; 

0100; 
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0010 ; 

0001 ]; 

% 

LINK3-TB0*T01‘T12»T23*T34; 

»3vec-[»3v«aJNK3(l,4) UNK3(3,4)]; 

«od 

uii([0 230-125 123]) 

plo«(«3vec(:.l)^vec(:^)) 

grul 

tiUcCKiiee2 WoilcspacO 

xUbelCX oooniaiate, cal’) 
yUbei^ ooacdinate, cm*) 
gtext(icNEE2') 
gtexl^3.6 defect') 
g|extC-lS6.4 degrees') 
glexl(TOOT) 
meUcd 

% Title: w2btniisjii 
% Inputs: (1A3,4.S.6.7A9) 

% Outputs: [1] 

fiPuipose: This fimctiaa tnnsfiimis world ooaniiaites to body coordinates. 

e ••««««««««*•••«««•««««««««««•«««««••••««••«***»•••«««•«•••***•«•«••*• 

% 

fimction [body] > w2btrans(phi, theta, psi, xtrans, ytrans, ztnns, wx, wy, wz) 

H 

%phi>rotatioaof{B} about the Xw-axis; 

% theta ■■ rotatioa of {B} about the Yw-axis; 

% pst =• rotatioa ci (B) about the Zw-axis; 

% xtrans > X-axis translation of {B} with respect to {W}; 

34 ytrans ~ Y-axis trsnslation of (B) with respect to {W}; 

%zlrans = Z-axisttaaslatiaoof{B} with respect to {W]; 

% wx = known Xworld coordinate; 

% wy - known Yworld coordinate; 

% wz « known Zworld coordinate; 

% 

% construct the translatioo vector, the body vector, and the rotation matrix 
trans = [xtram, ytrans, ztrans]'; 
world = [wx, wy,-wz, 1]'; 

rotate = [cos(psi)*cos(tfaeta) cos(psi)*sin(tbeta)*sin(phi)-siii(psi)*cosOihi) cos(psi)*sin(theta)*cos(piu>fsin(psi)*sin(phi); 
siii(psi)*c^tb^) sin(pti)*$i^thets)*sin(phi)+cot(psi)*cot(phi) sin(psi)*sin(th^)*cos(phi)-cos(psi)*siii(phi); 
-sin(th^) c^tb^)*sin(phi) cos(th^)*cos0>hi)]; 

% 

% build the body-to-world homogeneous matrix 
BWT = [rebate trans;0 001]; 

% 

% find the resultant 4X1 vector of equivalent body coordinates 
Tbodv = inv(BWT)*world; 

% 

% strip off the (x,y,z) coordinates 

body = [Tbody(l.l), Tbody(2,lX Tbody(3,l)]; 


% Title: work.m 
% Inputs: 

34 Outputs: woriemet plot 

% Purpose: This Matl^ program draws the UNCONSTRAINED workspace for 
34 AquaRoboL 

*««>«*<«**««****«**«**«*«**«***M*****««****«»*«*«******a*****«t*»**i 

% 

!del woriemet 

cig 

cic 

clear 

hold off 
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^oAnict body with radius 37 3 cm 
c-Q; 

1-37.5; 

*=(-37.3:0.5:37.31; 

fi)rin-l:kiiglh(x) 

y-sqrt(r^-x(:4n)^); 

c-(cjt(:jn)y]; 

end 

*-p7:-0.3:-37.3]; 

foriiPl:leiiglfa(x) 

y»-sqit(i^-x(:ji)^); 

C“(c-^:4>)y]; 

cad 

AxiiCsquafe') 

Axia(I~200 200 -200 200]) 

plot(c(:.l),c(:4)) 

grid 

hold 


% constnict legl 2D workspace 
<*= 0 ; 

x-37.5;0.5;200; 
forp=l:leiigtb(x) 
y=(1.73205*x(:j))>64.9519; 
d=‘{<tx(:,piy]; 
end 

plol(d(:,lX‘«(:.2Vg') 

% 

c=(l; 

*-37.3:0.3:200; 
for q=l:lenglh(x) 
y-(.1.73205‘x(:.q)>+^.9319; 
e“[epf(:.q)y]; 

end 

plol(e(:,lX«<:^2V«:) 

% 

% show legl 

Ll-0; 

x=37.3:0.3:178.2107; 

forq=l:lengtb(x> 

y-(0»x(:,q)>+O; 

U=[Ll;x(:,q)y]; 

end 

plot(Ll(:,l),Ll(:,2X'b’) 

% 

%showfootl 

x-178.2107; 

y=0; 

p!ot(x,y,'oc3') 

% construct Ieg4 2D workspace 

x=-180:0.3:-37.5; 
forr=l:Iength(x) 
y=(-1.73203*x(:^))-64.9519; 
f=[nx(:j)y]; 
end 

plot(fl::,l)J[:^XW 

% 

8 = 0 ; 

x=-200:0.5:-37.5; 

fors=l:length(x) 

y=(1.73205*x(:^))+64.9519; 

g=[g;x(:^)y]; 

end 

plot(g(:,lXg(;.2X’gO 

% 
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%]howleg4 

u-a; 

x--37.5:-0.5:-178.2l07; 
for q*=l:lcngtli(x) 
y-<0*x(:,q))+<); 

L4=(L4;x(:,q) y]; 

cod 

pIo«a4(;,lXL4(:.2X'b’) 

% 

% ihow foot4 
x=-178.2107; 
y^; 

plot(x,y,'oc5') 

r- I --. 

% construct leg2 2D workspace 

h=a; 

x=18.75;0.5;200, 
fort=l:length(x) 
y=<0*x(-..t)>32.476. 
h=[hyt(;.t) y]; 

end 

plol(h(:.l)Ji(:,2X'g^ 

% 

k=D; 

x=18.75:-0.5:-200; 
foru=l:lenglh(x) 
y=(1.73205*x(:.u)><4.9519; 
k=[k;x{;,u) y]; 
end 

plot(k(:,l)Jc(:^V80 

% 

% show Ieg2 

L2-D; 

x=18.75.0.J:89.1054; 
for q='l:lenglh(x) 
y=(-1.73025»x<:,q))+0; 
U=(Up<:.q)y]; 
end 

plot(U(;,lXL2(:.2).'b') 

% 

%sbowfoot2 

)C=89.1054; 

y=-134.335; 

plot(x,y,'oc5') 

% construct leg} 2D work^race 
1 = 0 ; 

x=-18.75:O.J:200; 
for v=l :length(xt 
y=(1.73205*x(;,v))+«4,9519, 

l=[lpc(;,v) y]. 

end 

plot(l(:,lXl(:.2X'g^ 

% 

m=[l; 

x=-18.75:-0.5:-200; 

forz=l:length(x) 

y=(0*x(:^)>+32.476; 

ni=[npc(:^)y]; 

end 

plot(in(:,l)^:,2),'g^ 

% 

% show legs 

L3=D; 

x=-18.75:-0.5:-89.1054; 

forq=l:length(x) 

y=(-1.73025»x(:,q))+0-, 






L5=[L5yt(:.q)y]; 

cad 

plo«(U(.,lXL5(:.2).t)') 

% 

%ihow foots 
)c--89.10S4; 
y-154.335; 
plot(x,y.'oc5') 

% coBstruct le^ 20 woricspace 
1 = 0 ; 

x=-18.75:0.5:200; 

forv=l:leiiglh(x) 

y-(-1.73205»x(:.v)><4-9519; 

l=0;x(:.v)y]; 

end 

plo«(l(:,lXl(:.2Vrt 

% 

m-Q; 

ic=-18.75:-0.5;-200; 
forr=l;lengtli(x) 
y=(0*x(:^))-32.476; 
m“{in;x(;;z) y]; 
end 

plot(ni(:,lX™(:,2Vg') 

% 

%showleg3 

L3-D; 

x=-18.75;-0.5:-89.1054; 

forq=l:length(x) 

y=Kl-73025»x(;,q)>+0; 

U=(Upc(:.q)y]; 

end 

plot(U(:,l),U(:,2),'b') 

% 

% show foots 
x=-89.1054; 
y=-154.335; 
plot(x,y,'oc5') 

% construct leg6 20 workspace 
1 = 0 ; 

x=18.75;-0.5:-200; 
forv=l;lengtli(x) 
r=(-l.73205»x(;,v))+64.9519; 
l=[l;x(:,v)y]; 
end 

plot(l(:,lXI(:.2),'gO 

% 

M=0; 

x=18.75;0.5;200; 

forz=l:length(x) 

y=(0»x(:^))+32.476; 

m=[m;x(:^)y]; 

end 

plot(m(:.lXni(:.2),'g') 

% 

%sbow Ieg6 

L6=0; 

x=18.75:0.5;89.1054; 
for q=l:Iength(x) 
y=(l-73025*x(:,q)>+0; 
L6=[L6;x(:,q) y]; 
end 

plot(L6<;,l),L6(:;i),'b') 

% 

% show foot6 
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x-89.1054; 

y=154.335; 

plol(x.y;oc5') 


% label plot 

titlefAquaRobot Uncooitniiied Leg Wofk^Mce') 

xlabclCX coordinate, cm*) 

ylabel^ coordinate, omO 

glexiCLeg One*) 

metawofk 






APPENDIX C: 


"C^” PROGRAM LISTING 


This appeadix contains the real-time gait planning code written in the ANSI 
language using the ATT version 3.1 compiler. These code modules are ready for 
implementation in the AquaRobot simulator (Silicon Graphics Personal Iris). These code 
modules, less the graphics portions, may also be used in the AquaRobot control computer 
(NEC 486-based Personal Computer) with slight modifications because of compiler 
differences. 
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// FILENAME: AbBotiy.C 

// PURPOSE: InvlanenUtiao of the AqiurobotBody class 
// CX>NTAINS: initializes the body fotm 

#iiiclude ’AbBotfy.H’ 

AquarobotBody:: AquarobotBodyO 

{ 

body_list > new matrix(7,4,0.0); // each row is a body point (x.y;t) 
//the first (0 row) is the body's 
// physical center, the rest are the six 
// peunts r^the body 

H_inatrix = new matttx(4,4,0.0); // the body's H-niatrix 

// the body design is constructed 
body_list->val(0,0) * 0.; body_list->val(0,l) = 0.; 
body list->val(0J) • 0.; body_list->val(0,3) = 1.; 
body~list.>val{l,0) = 37.5; body_list->val(l.l) = 0.; 
body litt->val(l,2) = 0.; body_list->val(l3) !•; 
body“list->val(2,0) = 18.75; body_list->val(2.1) = 32.48; 
body_list->val(2,2) • 0.; body_list->vsl(23) = l-l 
body_tist->val{3,0) - -18.75; body_list->val(3,l) = 32.48; 
body_tist->val(3,2) * 0.; body_list->val(33) = !•; 
body_list->val(4.0) = -37.5; body_list->^(4,l) “= 0.; 
body liat->vsl(4,2) *■ 0.; body list->val(43) = 1.; 
bodyjist->val(5,0) = -18.75; body_list->val(5,l) = -32.48; 
body list->val(5,2) = 0.; body_list->val(5,3) =1.; 
bodyjist->val(6.0) = 18.75; body_list->val(6.1) = -32.48; 
body_list->val(6,2) * 0.; body_list->val(6,3) = 1.; 

//defines the initial location of the body using the 
//H-matrix the inputs to the fijnetion ate: 

// (azirmith, elevation, roll, x, y, z) 

// H_matrix->HoinogeneousTransfbnn(0.,0.,0.,0.,0., -54.1819); 
H_rr»atrix->HoniogeneousTransfonn(0.,0.,0.,0.,0.,-70.7107); 

//moves the body to the initial location desired 
body_list->TrinsfotmBodyList(*H_tnatrix,*body_lis*); 


) 

void AquarobotBody.'.'MovelncrementaKdouble delaz, double delel, 
double deirol, double delx,double dely, double delz) 

{ 

double az, el, to, X, y, z; 
az - azimuth delaz; 
el = elevation -t- delel; 
ro = roll + deirol; 

X = body_list->val(0,0) + delx; 
y = body_list->val(0,l) + dely, 
z = body_list->val(0,2) + del^ 

// changes only are used since body_Iist is at current position 
H_inatrix->HoinogeneousTransfann(delaz, delel, deirol, delx, dely, delz); 
body_list->TransfonnBodyList(*H_matrix,*body_list); 

// puts all info in H_fflatrix 

H_inalrix->HoinogeneousTransfonii(az,el,ropc,y,z); 

} 

//»«*.«***•*»**•*****••••••••**•*•***•*••*•*••••*•**•••••«•*• 

// FILENAME: AbBody.H 

// PURPOSE: Declaration of AquarobotBody class 

// Subclass of RigidBody class 

//*»•****»**•*•••••••*••**••••»•*••***••***«*•••*••*•«•«••**• 
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#ifedrfH_AQUAROBOTBODY 
Mefioe H_AQUAROBOTBODY 


^iDclude <itdio.h> 
^include "linlclH" 
^include "AbRigidH” 
#include''Liiik.H'' 
#incliide "MathxMy.H” 


class AiiuarobotBody; public RigidBody 

{ 

pubUc; 

matrix *body_list; // defines the size of the body using coordinates 

matrix *H_mxtrix; // defines the location for body 
// using azimuth, elevation, roll, x, y, z 
double azimuth, elevation, roll; 

AquarobotBodyO; //constructor 

void Movelncreinental(doubie,double, double, double, double, doubteX 


}; 


#endif 

// FILENAME; AbLeg.C 
// PURPOSE: In^lementation of Aqual^eg class 
// CONTAINS: AquaLegQ 
// Ioitialize(AquaLeg&, AquatobotBody&) 

// TakePicture(Cainera&, AquaLegA) 

// Moveincreniental (Aqualeg&, deltal,delia2,delta3) 

#include "AbLegH" 

// FUNCTION: -AquaLegQ 
// PURPOSE: destructor of AquaLeg class 

AquaLeg: :-AquaLegO 

{ 

delete linkO; 
delete linkl; 
delete linlc2; 
delete Iink3; 

} 

// FUNCTION: AquaLeg 
//PURPOSE; constructor of AquaLeg class 
//RETURNS; AquaLeg class with values 

//»**••*•*•**••••**•**••**••***«*••••*•«••••«•••***•*•' 

AquaLeg::AquaLeg!AquarobotBody &body, double angle) 

{ 

motion_complete_flag = 1; 

SetLegAttachmemAngle(angle); 
linkO = new LinkO; 
linkl =new Linkl; 

Iink2 = new Link2; 
link3 = new Link3; 

// initial link values initialized 
matrix temp; 

tefnp.Ui.'dateTMatrix(GetLegAttachmentAngleQ,0.,O.,O.); 
temp = ‘b )dy.H_matrix • temp; 

linkO-'RotateLink(&tenq> ,linkO->GetInboardJointAngleO); 











liiikl->RoUteUiik(liiikO->H_matrix, linkl-XjeUnboardJomtAogleO); 
liiik2->Rot»tfl inkQinlf l->H_in»trixJiiilc2-><jetlnboafdJ<MntAiigleO>, 
liiik3^IloUteLiiik(liiik2*>H_nwtnxJiiik3->GetIiiboard >oiatAi]£ieO); 

} 

// nJNCnON: MovelncronenUl 

//PURPOSE: calculate the new link viliMs as a leg rotites 

//RETURNS; rotated link'k new values 

U ««««*««««**«««*•««••«««•««*«•««•«««««««•««•••••«««««•«•••••« 

void Ai]uaLeg;;MoveIncTemental(Aquarol)olBody dtbody double deltal, 

double detta2, double 

deltas) 

{ 

double b; 

// set all limit flags to zero 
linlcl>>SetMotionLiniitFIag(0); 
link2->SetMotionLiniitFlag(0); 
liok3->SetMatianlinutFUg(0); 

//temp matrix adds in the T_matrix needed for the physical 
//attadanent of the leg to the body 
matrix temp; 

ten9.UpdateTMatiix(GetLegAttadimentAngleO,0.,0.,O.X 

temp = *body.H_matrix * temp; 


b = deltal + linkO->GetInboardJointAngleO; 

linkO->SetInboardJointAogle(b)-, 

liokO>>RotateLink(&temp,linkO->GetInboardJoiatAngleO); 


b = delta2 + linkl->GetInboardJointAogleO; 
linkl->SetInboardJointAngle(b); 

liakl->RotateUolc(lmkO'>H_matrix,linlcl->Get]nboardJointAn^eO); 
b = deltas + this->link2-X5etInboar(lIointAngleO; 

Uiik2->SetInboardJointAngle(b); 

li]ik2->Rotate(linlc l->H_matrix,link2->GetInboar(UoiiitAngleO)l 

b = deltas + this->linlrS>>GetInboardJointAngleO; 
linkS>>S<tIoboardJointAngle(b); 

IiiikS->RoUteLink(linl(2'>H_matrix,linlcS->GetInboardJointAngleO); 

//themotion_coniplete_flagissetto 1 ifthe 
// nK>tion_linut_flags on all legs are not set 

SetMotionConq>leteFtag(!(thisc>linkl->GetMotionLiniitFlagO S 

th's->link2-><3etMotionLimitFUigO i 

this->link3->GetMotionLiinitFlagO)); 


// prints the status of the requested motion and prints which 
// link's motion_liinit_flag was set ('f any), 
if (GetMotionCompleteFlagO — 0) { 

printiCMotion Not Cofiipleted\n''); 

if(linkl->GetMotioiiLiinitFlagO ” 1) 

printfClink 1 limit exceedethn”); 
if Gink2->GetMotionLimitFUgO = 1) 

ptiiitl("Iink 2 limit exceedetbn"); 
if (linkS->GetMotionLiiiutFUgO 1) 

printfl^link 3 limit exceededln”); 

} 

// else printfCMotion completedin'O; 


} 


//**».***«.*.•*••**•**•**»***.*••**•«*•» 

//FILENAME; AbLeg.H 

// PURPOSE: Declarations for AquaLeg class 
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// 

// COMMENTS: Definitioo o( Aqualxg class and fimctions that 
// apply to this class 

#ifodefH_AQUALEG 
^define H_AQUALEG 

#inchide ’AbBody.H' 

#iiicliide "Unk.H” 

^include TinkOH” 
ttinclude ’Lmkl.H'' 
ttinclude 'Luik2.H' 
tHinclude ’LinlO.H' 

class AquaLeg 

{ 

public: 

LinkO *liiikO; // a LinkO class is instantiated 
Linkl *linkl; II a Linkl class is instantiated 
Link2 *liiik2; // a LinkS class is instantiated 
Link) *link3; // a Link3 class is instantiated 
int inotion_coaq)lete_flac//the flag is set to 1 ifthe motion 
// was cooqtleted without reaching any 
// link limits. 

int leg_suppa(t_flae //the flag is set to 1 ifthe leg 
// is on the floor (z - 0). 

double leg_attachiiient_angle; // the angle off of leg one where the 
//leg is attached to the body 

double previous_foot_x_coord; // saves last foot position 
double previous_foot_y_coord; 
double pievious_foot_z_coord; 

AquaLegfAquarobotBody double); // constructor and initializer 
-"AquaLegQ; // destructor 

double GctLegAttacfamentAngleO { return leg_attachment_angle;> 

int GetMotionCompleteFlagO { return motion_complete_flag;} 

void SetLegAttacfamentAnglefdouble angle) {leg^attachment_angle » angle;} 

void SetMotionCoaq>leteFlag(int flag) {motion_coinplete_flag == flag;} 

int GetLegSuppoitFUgO { return leg_support_flag;} 

void SetLegSuppoctF1ag(iiit flag) {ieg_support_flag ^ flag;) 

double GetPleviousFootXCoordO { return previous_foot_x_coord;} 

void SetPreviousFootXCoord(double xcoord) {inevious_foot_x_coord = xcoord;} 

double GetPreviousFootYCoordO {return ptevious_foot_y_coord;) 

void SetPteviousFootYCoord(double ycoord) {previous_foot_y coord = ycoord;) 

double CetPreviousFootZCoordO { return pievious_foot_z_coord;) 

void SetPreviousFootZCoord(dooble zcoord) {previous_foot_z_coord = zcoord;} 

void MoveIncremental(AquarobotBody &,double dettal,double delta2, 

double deita3); 

}; 

#endif 

//*«*«»•*•»«•***•***««•«•**•••••••••*•••••*••***•***•* 

// FILENAME: AbRigirLC 

// PURPOSE: Implementation of class RigidBody 

//CONTAINS: 

//**«,**.**«*****.***..*•***•.««*•**.•*•*•••****••*•••• 

#include "AbRigidH” 

// FUNCTION: RigidBodyO 










RigidBody: :RigidBodyO 

{ 

iiode_Iut ~ new ina&u(8,4,0.0); 
H malrix ~ new iiiatrix(4,4,0.0X 


// FUNCTION: Update Velocity 
//PURPOSE: 

//COMMENTS: 

n 

void RigidBody;:UpdateVek>cit>(doiiUe delta_t) 

{ 

u » u + (delu_t • udot); 

V = V + (delta_t • vdot); 
w = w + (deha_t * wdot); 
p = p + (delu_t • pdot); 
q » q + (delta_t • qdot); 
r * r + (delta_t • rdot); 

) 

// FILENAME. AbRigiAH 
//PURPOSE; 

// 

//COMMENTS: 

//•*••««««•**•••*•«••*••••••*«•«*••******•••••*••••*•*• 

#ifiridefH_RIGIDBODY 
Mkfine H_RlGIDBODY 

iHiiiclude <tiine.h> 

//#iiiclude''Uiiic.H'' 

#iiKlude 'MatnxMy.H' 

const double gravity •= 32.218S; 


class RigidBody 

{ 

private: 

// double location; 
double tt. y> z; 

// velocityfd]; 
double u, v, w, p, q, r, 

// acceleration[6]; 

double udot, vdot, wdot, pdot, qdot, rdot; 

// fi>rces_and_torques(6]; 
double Fx, Fy, Fz, L, M, N; 

// inoinents_of_inertia[3]; 
double I]^ ly, hi 
double mass; 
long curTeflt_tiine; 

public: 

matrix *iiode_list; 
matrix *H_matrix; 

RigidBodyO; 

~RigidBodyO{}; //more needed? 
void TransfonnNodeListO; 

/• 

// Use these for dynamics::::::: 

matrix& TranslateAndEulerAngleTratisform (double x, double y, double z. 
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double azunuth, 
double clevitiaa, 
double roll); 
double GelDelUT 0; 
void StaitTimer Q; 
void UpdateRigidBody 0; 

•/ 


void l^MlateAccelefatioB Q; 

void Update Velocity (double delu_t); 

/* 

void UpdateHMatrix (double delta_t); 
void UpdatePocitiaa 0, 
void CetNodePolygoaList Q; 

•/ 


void SetU(double a) {u - a;} 
void SetV(double a) {v = a;} 
void SetW(double a) (w - a;) 
void S^P(double a) (p - a;} 
void SetQ(double a) (q = a;} 
void SetR(double a) (r = a;} 

double GetUQ {retuni u;} 
double GetVQ (return v,} 
double GetWQ (return w,} 
double GetPQ (return p;} 
double GctQO (return q;} 
double GetRO (return r,} 

void SetUdot (double a) (udot = a;} 
void SetVdot (double a) (vdot« a;} 
void SetWdot (double a) (wdot = a;} 
void SetPdot (double a) (pdot ° a;} 
void SetQdot (double a) (qdot > a;) 
void SetRdot (double a) (t^ a;} 

double GetUdotQ (return udot;} 
double GetVdotO (return vdot;} 
double GetWdotO (return wdot;} 
double GetPdotO (return pdot;} 
double Gt.< JdotQ (return qdot;} 
double GetRdotQ (return rdot;} 

void SetFx(double a) (Fx = a;} 
void SetFy(double a) (Fy = a;} 
void SetFz(double a) (Fz = a;} 
void SetL(double a) (L = a;} 
void SetM(double a) (M ° a;} 
void SelN(double a) (N ”= a;} 

double GetFxQ (return Fx;} 
double GetFyO (return Fy,} 
double GetFzQ (return Fz;} 
double GetLO (return 1;} 
double GetMO (return M;} 
double GetNO (retL n N;} 

void Setlx(double a) (lx = a;} 
void Setly(double a) (ly = a;} 
void Setl^double a) (Iz = a;} 

double GetlxQ (return Ix;} 
double GetlyO (return ly,} 
double GetlzQ (return Iz;} 
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#cndif 


//rrrLE: atcinLC 

//INPUT: (x,y)body coordinates of* sdecled foot; dircctioo; body cotter. 


// ndius of wotlcspace; CCW arc aegnieot endpoints; CW arc segment 

// em^wints; foot nunfoer selected 

//OUTPUT: intercepts ofarc segment; distance to the intercepts fom the 

// cunent foot position; flag to indicate whether an irttercept 

// was found 

//FUNCTION: This fimetioo determines whether there is an intercept of the 
// desired direction offoe foot widi an arc segniditdefirimg a part 

// ofthewotfomace. 

//**«*****«**«*«*«««*«*.^UAR0B0T**«*******«**********************< 


ttinclude <stdio.h> 

#include <mathJi> 

#definePI3.141S926d3» 

int arciitt(double foot[], double direction, double cenbod[], double radius, double i)eg_liinit(], double pos_liinit[], ittt foot_nuinber, 
double interoeptsQ, double &diataiice) 

{ 

// initializatian 

double a, b, c, d, x_itttercept, y_intcrcept; 

double perp_dista n c e , len^ x_pc>P, yjxtp; 

double rad_s(|uared, petp_squared; 

double radius_test, in_directioo, out_directioo, footnd; 

double atc_foat, neg_arc_liniit, pos^arejimit, limit > 21 S. 0 ; 

int arcsegment_flag > 0 ; 

//determine if foot foils on the arc segment 
fooUttd ” S 4 rt( (foot{0]*foot{0]>+(foot[l]*foot{l]) y, 
if (radius = footrad){ 

arcse 0 tKstt_flag - I; 
ittteroepts[di > foot[ 0 ]; 
interoepts[l] -foot(l]; 
distance «limit; 

} 

if (arcsegtnettt_flag 0 ){ 

perp_distance = ((oenbod[4] • foot[l])*cos(direction)) 

- ((oeobod(3] - foot[0])*sin(direction)); 


tf (potp_distance <= radius){ 
radjsquared = radius*rsdius; 
petp_squared = perp_distance*perp_distance; 
if (petp_distance < 0 ){ 

lengfo = sqrt (-(rad_squared - perp_s<]uared»; 

} 

else { 

length = sqrt(rad_squated - perp squared); 

} 

// fiixl (x,y) at intersection of petpindicular distance and length 
x_pop = cenbod[3] + (perp_distance*sin(direction)); 
y_perp = cenbod[4] - (petp_distance*cos(direction)); 


// test if foot is inside or outside radius 
a = (foot[l] - cenbod[4])*(foot[l] - cenbod[4]); 
b = (foot[0] - cenbod[3])*(fooit[0] - cenbod[3]); 
radius_test = sqrtfa + b); 

if (radius_test > radius){ 

// find (x,y) at intersection of ray and arc if foot is outside radius 
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x_atttoept - x_pap - (leiigtb*cai(<lirectioo)X 
y_mlcrc«pl = yjpeip - (leog|h*sin((b<ctiao)); 
intcrecpU(0] - x_iBlcrcc|]t; 
iiitaoq>tt[l] = y_aileree|it; 

} 

else { 

// (x,y) intcnectioo of ny and arc if foot is intide radius 

x_iiiic(cept - x_pctp + (Ieaglh*cos(directioa)); 
y_iiitcrce|it > y_pcn> (lenglh*sin(directioa)); 
iiileroq)la[0] - x_iolerce|)t; 
iiitCTcepls[l] - y_ioiercepC; 

} // radius test cods here 

//test if intersectioo is in desired directioa 

in_direction = foot{0]*cae(dircction) foot{l]*sin(direction); 

out_dir6CtioD - x_iiiiace|it*cos(directioo) + y_iidercept* s in(direction); 

if (in_directioa <* out_difeGtioa}{ 
c ” (y_intercept - fi)ot[l])*(y_!nteroept - foot(l]); 
d = (x_intercept - foot{0])*(x_inlercefit - foot[0]); 
distance > sqit(c + d); 


//test if intersection is within arc segment 

arcjbot = atan2(y_iEtercept - cenbod[4], x_intercept - cenbod(3)); 
neg_arc_limit = atan2(neg^ltmit(l] - cenbod[4], neg_liinit[0] - cenbod[3]); 
pos_arc_liniit =• stan2(pos_limit[l] - cenbod[4], pos_timit[0] - cenbod[3]); 

//tests to ensure proper sections of arcs are used 
if (foot_number == 1){ 

if (arcJToot >= oeg^arc_li 0 iit && arc_foot <= pos_arc_linut) 
arcsegment flag= 1; 

} 

else ( 

if(direction>0){ 
if (neg^arc_Ui^ < 0) 
neg;_arc_liniit = neg;_arc_liniit + (2*PI); 
if (pos_arc_limit < 0) 
pos_atc_limit = pos_arc_limit + (2*PI); 
if (atc_foot < 0) 

= arc_foot + (2*PI); 

if (arc_foot >= neg_arc_limit && arc_foot <= pos_arc_Umit) 
aicsegmoitflag = 1; 

} 

else { // directioa < 0 
if (neg_arc_limit > 0) 
neg_arc_iimit = neg_are_limit - (2*PI); 
if (pos_arc_limit > 0) 
pos_arc_limit = pos_arc_lirait - (2*PI)', 
if (arc_foot > 0) 
arc_foot = arc_foot - (2*PI); 

if (aic_foot >= neg_aic_liniit && arc_lbot <= pos_afc_liniit) 
arcseginent_flag = 1; 

} // direction test ends here 
} //£x>t number test ends here 
}//intersection test ends here 
} // perpindicutar test ends here 


if (arcsegment_flag != 1){ 
atcsegmeiit_flag = 0; 

distance = limit; 
intercepts[0] = x_intercept; 
intercepts[l] * y_irtercept; 


} // nullifying parameters ends here 
return (atcsegmeiit_flag); 
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} // arant fimctioa endi beie 


/»»—«»»«»««««»««««««««««»»»«»««»««»»»«»««»«»«»«»»»««»«»»««»««»«««««»»» 

FILENAME: ucomLH 

PURPOSE: Header file. This file leptcfcalt AquaroboCs coaftaat paramden 
COMMENT: 

#ifiidef_ARCONST_H 

#defiiie_ARCONST_H 

Mefine PI 3.1415926S3589793115997963468544183161590576171875000 

#defineHPI(PI^.O) 

#<lefiiie DR (PI/180.0) 

Mefine ROOT3 1.732050807368877193176604123436845839023590087890625 
Mefine EQU_TR1ANGLE 3.0/2.0 

#defiiieL£G6 
#<lefiiieL£Gl 0 
#<iefineLEG2 1 
#defiDeLEG3 2 
#<lefiiieLEG4 3 
MefineLEG5 4 
MefuieLEG6 5 

#<lefiiieCBO 
#define HIP 1 
#(lefine KNEEl 2 
#(kfuMKNEE2 3 
#defiiieFOOT4 

#<»efiiieVJOINT4 
#defineJOINT3 
#defiiieJOINTOO 
WefineJOINTl 1 
#defineJOrNT2 2 
#<]efineJOINT3 3 
//define JOINT4 4 

#define LINKO 37.5 
#define LINKl 20.0 
#defineLINK2 50.0 
#define UNK3 100.0 

#define LINK02 (37.5*37.5) 

#defuie LrNK12 (20.0*20.0) 

//define UNK22 (50.0*50.0) 

/Mefine LINK32 (100.0*100.0) 

#derine MAXMIN 2 

#define TwoDun 2 
#derine ThreeDim 3 
//define XY 2 
#defineXYZ3 
#define EULER 3 
//define XW 0 
//define YW 1 
/define ZW 2 
#derine XB 0 
//define YB 1 
/MefineZB2 
/MefineXO 
//define Y 1 
/MefineZ2 
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Mefine RcsetThctel (0.0) 
Mefine RaetTbete2 (66.4) 
Me&BC ResctThele3 (-136.4) 

Mt&ut StaitThetaOLegl 0.0 
#define SUftTbetaOLe^ 60.0 
#<]efine Sta(tTheU0Leg3 120.0 
#<lefiiie Stait'nieU0Le84 180.0 
#defiiie StartTheUOLegS 240.0 
^define SUftT1)eU0Leg6 300.0 

Mefine StaitTheUl (0.0) 

#defiiie StaitTheU2 (33.86) 
#defiiie SuitTheU3 (-123.86) 

MeGstt TbetelNegLim (-60.0) 
Mefine Thetc2NegLiin (-106.6) 
Mefine Thete3NegLim (-136.4) 

^define ThetelPosLim (60.0) 
#define Thete2PosLim (73.4) 
MeBae Thete3PosLiin (23.6) 

#define StaitX 98.02 
#define StaitY 0.0 
#defiiie StaitZ 0.0 

#define TripodSize 98.02 
MeBae StartTripodSize 98.02 
^define DefauttSTRIDE 120.0 
#defiiie DefaultFOOTheight 13.0 

/* 

#defiiK HighHight 120.0 
UdeBne MtidiumHigbt 100.0 
MdeBne LowHight 80.0 

•/ 

#deiiiK InitialElpsRatio 2.0 

^define JlLimN (-60.0*DR) 
#defiiie JlLimP (60.0«DR) 
#de£ine J2LimN (-106.6*DR) 
#defiiieJ2LimP( 73.4»DR) 
^define J3UmN (-156.4»DR) 
#defineJ3LimP( 23.6*DR) 

//^define ORIENTATION 3 
#deiine AZIMUTH 0 
^define ELEVATION 1 
UdeBme ROLL 2 

Adeline YAW 0 
#define PITCH 1 
#defmeROLL2 

/• 

^define AZIMUTH 3 
#define ELEVATION 4 
#defme ROLL 3 

#defme YAW 3 
#definePITCH4 
#define ROLL 3 
*1 









#ifiidef arfi;nc_c 

#define_ARFUNC_C 
// FILENAME: ufunc-C 

// PURPOSE: Basic Mathematical Functioos for Aquarobot Simulation. 

ttinclude <stdio.h> 

#ioclude <inath.h> 

#include ’arconstH" 

#include 'Kinematics-H" 

//TITLE: minO 

// FUNCTION: minimum operation 
//INPUT: 

//OUTPUT: 

//RETURN: 

//COMMENT: 

//DATE: 

double min(doubIe x, double y) 

{ 

iS.x<y) 
return x; 
else 

return y, 

} 

// TITLE: maxO 
// FUNCTION: maximiun operation 

double max(double x, double y) 

{ 

il(x>y) 
return x; 
else 

return y, 

} 

// TITLE: ellipseO 

// FUNCTION: Calculates incremental foot point along an elliptical path 
// generated between the last step and the next step. 

//INPUT: 

//OUTPUT: 

// CALLED BY: tripodOphaseO, tripodlphaseO, bodyphaseQ. 
//COMMENT: Tte function is for continuous motion. 

// This function divides ellipse angle into FINE. 


void ellipsefWallcParameter &wp, // walking parameters 

double footprintQ, // 
double footpointQ) // 

{ 

double theta; // ellipse angle 
double d_theta; // dumge in ellipse angle 
// double d_segnien^ // ellipse segment speed 
double a, b; // x_diameter, y_diameter 
double x, y, z; // ellipse coordinates 


a = wp.stride / 2.0; 
b = wp.footheight; 

/• Coordinates Transformation(WORLD->ELLIFSE) */ 
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X - cot(wp.diiectioa*DR)*(footpoinipCW] - footprintpCW]) 

+ sin(wp.(lirectioo*DR')*(footpoiiit[YW] - foa(iinnt[YW]); 
y =- (foo4)omt[ZW] - foot|iirint(ZW]); 
z =- siii(wp.<iiicctioo*DR)*(fiM<poiiitixW] - footpriat(XW]) 

+ ca«(>wp.(it(ectioii*DR)*(foatp^[YW] - foolprintfYW]); 

/* to olculite cumnt ellipie angle */ 
theta » atan2((y/b), (x-aya); 

d_theU * PI/FINE; 

/* decrement ellipse angle */ 
theta — d_theta; 


/* to calculate next foot point */ 

X = a*(1.0 + cos(theta)); 
y =• b*sin(theta); 
z = 0.0; 

if(theta<=0.0){ 

thett= 180.0; 

} 

/* to calculate foot position on World coordinates */ 

footpointpfW] = x*oos(wp.directioo*DR) - z*sin(wp.directioo) + footprintpOV]; 
footpointfYW] » x*sin(vvp.direction*DR) + z’co^wp.direction) + footprintfYW]; 
footpoint(ZW] = - y + foolprintlZWj; 


} 

// TITLE: kinematicsQ 

// FUNCTION: Computes Kinematics for .\quarobot Type II. 

// INPUT: Joint An^es(1heta[CB->FOOT] Unit:degree). 

// OUTPUT: Joint Positions related to BODY coordinates(Unitcm). 

H CALLED BY:mv_kjnenuticsO, tripodOphaseO; tripodlphaseO. bodyphaseO 

void kinematics(double thetaQ, // Joint Anglt. 

double hip[], //HIP Joint Position(BODY) 
double kneelQ, // KNEEl Joint Position(BODY) 
double knee2[], // KNEE2 Joint Pasition(BODV) 
double footQ) // FOOT Joint Positioo(BODY) 

{ 

double cO, sO, cl, $1, c2, s2, c3, s3; 
double cOl, sOl, c23, s23; 
inti; 


foifi=0; i<4; i++){ 
th^[i] == theU[i]*DR; 

} 

cO = cos(tlieta[0]); $0 = sin(theta[0]); 

cl = co$(theta[l]); si = sin(thela(l]); 

c2 cos(theta[2]y s2 = sin(theta[2]); 

c3 = cos(thetaP]); s3 = sin(theta[3]); 

cOl = co^theta[0j-Hbeta[l]); sOl = sin(theta(0]-»'theta[l]); 

c23 = cas(theta[2]-Hheta[3]); s23 = sin(theta{2]+dieta[3]); 

hippCB] “ LINK0*c0; // HIP Position 
hip[YB] = UNK0*s0; 
hip[ZB] > 0.0; 

kneeipffi] - hippCB] + UNKl»c01; // KNEEl Position 
lcneel(YB] =hip(yB] + LlNKl*s01; 
kneel[ZB] = 0.0; 

knee2pCB] = kneelfXB] + LINK2*c01*c2; // KNEE2 Position 
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kiiee2[YB]' kneel[YB] + LINK2*s01»c2; 
kfiee2[ZB] ^UNK2*s2; 

footpffl] = taiee2[XB] + LINK3*c01»c23; // FOOT Positioo 
foot(YB] = kiiee2[YBJ + LINK3**01*c23; 
foot(ZB] - k]iee2[ZB] - LINK3*s23; 


i<4; i++){ 

tb^i] > theU(i]/(DR); 

> 

} 


// TITLE: inv_kiiiematicsO 

// FUNCTION: Computes Inverse Kinematics for Aquarobot Type II. 

// INPUT: FOOT position in World Coon]inates(footpCYZ]). 

// OUTPUT: Joint Angles(Unitdegree). 

// CALLED BY:tiipodOpliaseO, tripodlphaseQ, bodyphaseQ 
// CALLS: kinem^csO 

void inv_kinematics(doubIe footQ, // FOOT Position(BODY) 

double thetaflV/ Joint Angle 

{ 

double px, py, pz; 

double px2,py2.pz2; /• px^, P!^ pz^ •/ 

double xO, yO, zO; /* CB coonlinates origin */ 

double cO, sO, cl. si; /* sin(theta_i), cos(tbetaJ) */ 

double c3, s3; /* sin(theta_i), cos(thetaJ) */ 

double cOl, sOl; /* si^theta_i+thetaJX cos(thte_i+tlietaJ) */ 

double bl.b2.b3; /* length bi*/ 

double bl2.b22,b32; /*bi'^»/ 

double beta; /* angle beta */ 

double cpsi, spsi; /* cos(psi), sin(psi) */ 

double theta2j), theta2_n; 

double theta3_p, theta3_n; 

double fla^ 

double 4(4]; !* joint angle •/ 

double l9[3], lcl[3], k2[3], ^3]-./* joint position *! 
inti; 

px = foot(0]; px2 = px*px; 
py « foot[l]; py2 = py*py, 
pz => foot[2]; pz2 = pz*pz; 

theU[0]=tbeU(0]*DR; 
cO = c^theta(0]); 
sO = sin(tlieta[0]); 

bl2 = (px-LINKO»cO)*(px-LINKO*cO) + (py-LINKO’sO)*(py-LINKO»sO); 

bl =sqrt(bl2); 

b2 =bl-LINKl; 

b22 - b2*b2; 

b32 = b22+pz2; 

b3 =s<jrt(b32); 

/•theUl •/ 

cl = ( px2+py2-LINK02-bl2 y( 2.0»LINK0*bl ); 
cl =niiii(1.0, cl); 
si "■sqi1(1.0-cl*cl); 

/* position invetse transformation from CB to JO coordinates */ 
xO = px*cO + py*sO; 
yO =-px*sO + py*cO; 
zO=pz; 


ifr:x0<=0.0){ 
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ptintfC##### XB range ennr #####«'); 

> 

ifl; yO >“ 0.0 ){ 
thi^l] atiui2( si, cl); 

} 

ebe{ 

tb^l] = aUn2(-«l, clX 

> 

if( (theU[l] < JlLtmN) && (theta{l] > JlLimP) ){ 
printfT##### NO SOLUTION FOR THETAl (WHWWftn*); 

} 

cOl => cas(theU[0]-Kheta[l]); 
sOl = $in(tbeta{0]-i-OieU[l]); 
theU(0]>theU[0]/(DR); 
theU[l]=°theta{l]/(DR); 

/•theta2*/ 

beta atan2( pz, b2); 

cpsi * inin(1.0. ( UNK22+b32-LINK32 )/(2.0*LINK2‘b3 )); 
^ - s<pt(1.0-cpsi*cpst); 


theta2_p = (atan2( q)si, cpsi) - beta)/(DR); 
tbeta2_n >= ( atai)2(-^ist, cpsi) - beU)/(DR); 

/•theta3»/ 

c3 = iniii(1.0, ( b32-LINK22-LINK32 )/(2.0*UNK2‘LINK3 )); 
s3 = sqrt<1.0-c3*c3); 
theta3_p aUn2( s3, c3)/(DR); 

1heta3_n = atan2(>s3, c3)/(DR); 

/* selection afproper combination of ttieta2 and theta3 */ 

th(0]=tlieta[0]; th[l]=theta[l]; 

flag^; 

fot(i=0; i<4', i++){ 
svvitcfa(i){ 
caseO: 

th(2] = theta2j>; th(3] = theta3_ii; 
break; 
case 1; 

th[2] =theta2 n; th[3] =theta3_n; 
break; 
case 2: 

th[2] *lbeta2_n; th[3] = theta3_p; 
break; 
case 3: 

th[2] = theta2_p; th[3] = theta3_p; 
break; 

} 

kinematics(th, bp, kl, k2, ft); 
iff (fabs(px-ft[XB])<1.0e-3) 

&& (fabs(py-ft{YB]) < l.Oe-3) 

&& (fabs(pz-ft(ZB]) < l.Oe-3)){ 
theU[2] =1h[2]; 

1hela[3] =lh[3i; 

flag=l; 

break; 

} 

} 

ift;flag=0) 

printfi:"##### NO SOLUTION FOR THETA2 & THETA3 ##### \n"); 


//*•«****•*****•*•*•»»»***•**********••**.****.•.**.*.*«***« 

// TITLE: world_bodyO 

// FUNCTION: Cooidinate transfoimation fiom BODY to WORLD 
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//INPUTS; euleraiiglci(eulcr(p(i,theU^]XOngioafBOOYcoor<lioaies. 

// A2iinulh,ElevaliotUIoU 

// Yaw,Pildi.R<^ 

//OUTPUTS; WORLDooofdinata 

// CALLED BY iripodOphaseQ, tripodlphascO, bodyphaaeO 

void wcirld_bo<iy(double culcrQ, //Euler angles 

double orgQ, // Origin of BODY coordinates on WORLD 
double positionQy/ coorxiinales 

{ 

double cl, c2. c3, si, s2, s3; 
double X, y, z; 

cl ~ oosfeuletf AZIMUTH]); sl=°sin(euler[AZIMUTH)); 
c2 = co8(euler(ELEVATION]); s2=sin(euler{ELEVATIONl); 
c3 = co6(euler[ROLL]); s3^=sin(euler[ROLL]); 


X » positiaapCB]; 
y = positionfYB]; 
z = positioofzB]; 


poshionpCW]« cl*c2»x + (cl*s2‘s3.sl»c3)»y + (cl«s2*c3+sl*s3)‘z + ocg[X]. 
positknnrW] = sl»c2*x + (sl»s2»s3+cl»c3)»y + (sl*s2»c3-cl»s3)*z + ofg(Y]; 
pasition[ZW] = -s2*x + c2*s3*y + c2*c3*z + org(Z); 

} 

/'TITLE: body_worldO 

// FUNCTION: Coordinate transformation from WORLD to BODY 
// INPUTS: euler angles(euler[psi,thetaj)lii]). Origin of BODY coordinates. 

// Azimuth,Elevation,Rotl 

// Yaw,Pitch,RoU 

//OUTPUTS: BODY coordinates 

// CALLED BYiripodOphaseO, tripodlphaseO, bodypiuseO 

//t**.****.******...***.***.....**.**...**.*.*..*.**.*..*..***..*.**...* 

void body_world(double eulerQ, // euler angle 

double orgt), // origin of BODY coordinates on WORLD 
double positionQ) // coordinates 

double cl, c2, (3, si, s2, s3; /* cosQ, sinQ */ 
double X, y, r, /* WORLD coordinates */ 


cl>oas(euler{AZIMUTH]); sl=sin(euler[AZIMUTH]); 
c2=cos(euler(ELEVATION]); s2=sin(euler[ELEVATION]); 
c3>«Qs(euler(ROLL]); s3=sin(euI«piOLL]); 


X “ positionpCW]; 
y = positioo[YW]; 
z = position[ZW]; 


positioopCB] = cl*c2*(x-otgpCW]) + sl*c2*(y-oigPfW]) - s2*(z-oig{ZW)); 
poshionfYB] = (cl*s2»s3-sl»c3)*(x-orgpCW]) 

+<sl»s2*s3+cl»c3)*(y-orgrYW]) + c2*s3»(z-orgtZW)); 
posiUoo[ZB] = (cl*s2»c3+sl*s3)*(x-orgpCW]) 

+(sl*s2*c3<l*s3)^-otglYW]) + c2*c3»(z-orgtZW]). 

> 

#endif 

//TITLE: maxdistaiice_25cmfoot0 

//INPUT: (x,y)body coordinates ofall six feet and body center, tripod 
// mimbCT(0 = tripod0,2= tripod 1); direction 

//OUTPUT: maxtoHun stride possible without exceeding wori(q>ace 
//FUNCTION: This function finds the maximum stride possible for AquaRobot, 
// using a tripod gait, given an arbitrary dir^on as an input 

//***.*..*..*.*«...*.«.*y^QUAROBOT****************************»** 
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double maxdisuiice 2Sciiifool(Nc]a_Matioa ten. WaOcPenmeter Awp) 

{ 


int «rcml(double*, double, double*, double, double*, double*, im, double*, douUe ft); 
int aegiin(double*, double, double*, double*, double*, double ft>, 

// initulizaliaa 

double iond37.3; //imer radius of woik^Moe 

double outnd- 178.2107; //outer radius of workspsce 

double msxdist. distsnce_s, disUncs_b, disUacc_c, limit ~ 36.31; 

double mtetctpts{2]; 

double mdistsiioe ~ 0.0; 

int interccpt_flag == 0; 

int footnum, convert; 

double body_oenler{3]; 

double cul^3]; 

double sfoot(3]; 
double bfi>at[3]; 
double cfoot(3]; 
double dfoo([3]; 
double efoot(3]; 
double 9bot{3]; 
double oenbod(6]; 

for (convert = 0; convert <* 2; convert-*-*-) { 

aibotfeonvert] - nmi(>ot_l_coarid[convert]; 
bfoot[oonvert] - nmibot_2_coord[convert]; 
cfbot[oonvert] =• nnifoot_3_coord(coovert j; 
dfooC(oonvert] • anLfoot_4_coord{convert]; 
efoo((coavert] = nnLfoat_3_coord(coovertj; 
ffoot(ooiivert] « nm-foot 6 ooord(oonvert]; 

} 

for (convert » 0; convert <• 3; oonvert-*-*-) { 

oenbod[convert] ■ nm.body center ooord(ooovcrt]; 

} 


double dir ~ wp.direction * DR; 
iid tripod = vrp.phase; 


for(coovert = 0; convert 2; convert-*-*-) { 

euIer(coovert] = nm.body_center_ooocd(convert]; 
body_center[cQnvert] = nm.body_oenter coord(convert-*-3]; 

} 

static double seglpin[2] = {36.8686,6.8324}; 

//inside endpoint of CW segment #1 

static double seglpout(2] = {160.2049,78.0606}; 

// outside endpoint of CW segment #1 
static double seginin[2] ° {36.8686, >6.8324}; 

// inside endpoint of CCW segment #1 

static double segliiout(2] {160.2049, -78.0606}; 

// outside endpoint of CCW segment #1 

static double seg2pin[2] > {12.3,33.3333}; 

// inside endpoint t^CW segment #2 
static double seg2pout{2] ^ {12.3,177.7718}; 

//outside endpoint of CW segment #2 
static double seg2nin[2] ^ {24.3686,28.3030}; 

// inside endpoint of CCW segment #2 

static double seg2nout[2] = {147.7049,99.7112}; 

//outside endpoiin of (XW segment #2 

static double seg3pin[2] > {-24.3686.28.3030}; 

//inside endpoint of CW segment #3 

static double seg3poot(2] = {-147.7049,99.7112}; 

// outside endpoint of CW segment #3 
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static doubUseg3mii[2]- (-12.5,3S.3SS3}; 

//inside codpoiiit of CCW sc^nent #3 
static double seg3noiJt{2] • {>12.3.177.7718}; 

//outside endpoint of CCW sepnot #3 

static double seg4pin(2] > {-36.8«86, -6.8324}; 

//inside endpoint of CW segment #4 

static double seg4pout[2] - {-160.2049, -78.0606}; 

//outside endpoint of CW segment #4 

static double seg4iun(2I (-36.8686,6.8324}; 

// inside endpoint of CCW sepnent #4 

static double seg4iiout{2] = (-160.2049,78.0606}; 

// outside endpoint of CCW segment #4 


static double seg3pin[2] = (-12.3, -33.3333}; 

//inside mdpoim of CW segment #3 

static double seg3pout[2] = (-12.3, -177.7718}; 

// outside endpoint of CW segment #3 

static double seg3nin[2]» (-24.3686, -28.3030}; 

//inade endpoint of CCW segment #3 

static double seg3nout[2] ^ (-147.7049, -99.7112}; 

// outside endpoint of CCW segment #3 

static double seg6pin[2] > (24.3686, -28.3030}; 

// inside endpoint of CW segment #6 
static double seg6pout[2] = (147.7049, -99.7112}; 
// outside endpoint of CW segment #6 
static double scg6nin(2] = {12.3, -33.3333}; 

// inside endpoint of CCW segnient #6 
static double seg6oout[2] = (12.3, -177.7718}; 

// outside endpoint of CCW segment #6 

// convert from WORLD to BODY ooofdinates 
body_world(euler, body_center, afoot); 
body_vwrld(euler, body_center, bfoot); 
bo<<y_world(euIer, body_ccn(er, cfbot); 
body_worid(euler, body_oenter, dfoot); 
body_\vorid(euler, body_cenler, efoot); 
body_worid(euler, bod>_center, ffoot); 
body_worid(euler, body_center, &oenbod[3]); 

if (tripod = 0){ 


// test footl in leg 1 woricspace 

foocnum = 1; // select foot number one 

// test for intersection with outer workspace limit 

intercept_flag » arcintfafoot, dir, cenbod, outrad, seglnout, seglpout, footnum, intercepts, mdistance); 
if (intercept_flag = 1) 
distance_a => mdistance; 


// test for intersection with inner workspace limit 

intercept flag = arcintfafoot, dir, cenbod, inrad, seglnin, seglpin, footnum, intercepts, mdistance); 
if (mtercept_flag = 1) 
disunce_a = mdistance; 


n test for intersection with CW workspace limit 

interceptflag = segint(afoot, dir, seglpin, seglpout, intercepts, mdistance); 
if (intercept_flag = 1) 
distance_a mdistance; 

//test for intersection with CCW workspace limit 
intercept_flag = segint(afoot, dir, seglnin, seglnout, intercepts, mdistance); 
if (intetcept_flag == 1) 
distance_a = mdistaiKe; 


// test foot3 in leg 3 workspace 
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fgotDum « 3; // select foot Dumber three 

//test for inlersectiaa with outer workspace limit 

inleroept_flag - arcinl(cfoat, dir, oeriwd, outnd, 

segSnout, segSpout, footoum. inletcepls, mdi s t a nce ); 

if (inlsroept_flag "1) 
distaiioe_b - mdistaiim; 

//test for intenectko with bmer workspace limit 
inlsrcept_flag - afciiit(cfoot, dir, oenb^ innd, 

teg3niii, segSpin, footnum, intercqDs, mdistance); 

if (iDleroqit_flag “1) 
distaiice_b mdistaiice; 

// test for intefsectioii with CW workspace limit 
intercept_flag = segintfcfoQt, dir, segSpin, segSpout, 

intercepts, mdistanceX 

if (interoept_fUg “1) 
distance_b = mdistance; 

// test for intetseclion with CCW workspace limit 
imerccpt^flag >= tegifit(cfoot, dir, segSnin, segBnout, 

intercepts, mdistance); 

if(interoept_flag — 1) 
distance_b mdistance; 

// test foot5 in leg 5 workspace 
footnum = 3; // select foot number five 
// test for intersection with outer workspace limit 
iotercept_flag ° arcintfefool, dir, cenb^ outrad, 

segSnout, segSpout, footnum, intercepts, mdistance); 

if (inlercept_flag “ 1) 
distance_c > mdistance; 

// test for intersection with inner workspace limit 
intercept_flag = arcint(efoot, dir, cenbod, inrad, 

seg5iun, seg5pin, footnum, intavefMs, nuUsUitct); 

if (intercept_flag “ 1) 
distance_c ~ mdistance; 

// test for intersection with CW workspace limit 
intercept_ilag tegint(efoot, dir, segSpin, segSpout. 

intercepts, mdistance); 

if (iiitercept_flag “ 1) 
dislance_c - mdistance; 

// test for intersection with CCW workspace limit 
intetcept_flag ^ seguitfetboi, dir, segSnin, segSoout, 

intercepts, mdistance); 

if(ir*ercept_flag ” I) 
distance_c - mdistaiKe; 


maxdist = min(niin(distance_a, distanoe_bX distance_c); 


} //this ends tripod 0 testing 

else if (tripod “ 2){ 

// test foot2 in leg 2 workspace 

firotnum - 2; // select foot number two 

// test for intersection with outer workspace limit 

intercept_flag • arcint(bfool, dir, cenbod, outrad, 

seg2nout, seg2pout, footnum, intercepts, mdistance); 

if(intereept_flag“ I) 
distance_a ~ mdistaiKe; 

//test for intersection with inner workspace limit 
intercept_f1ag ~ arcint(bfoot, dir, cenbod, inrail. 
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leg^pin, footnum, intercepts, mdistance); 

if(inlei«ep«_fl«g— 1) 
disUace_s - mdtsUnce; 

//test for mtcnectioa with CW workspace bmit 
ii]taoept_fUg > scgiotfhfoot, dir, se^pin, seg2pout, 

intercepts, mdistance); 

if (intcrccpl_flag •= 1) 
distance_a ~ mdistance; 

//test for iidenection with CCW woikspace limit 
intercept_flag > segiiil(bfoot, dir, segZnin, segZnout, 

intercepts, mdistance); 

if (intetcept_flag “ 1) 
distance_a > mdistance; 

//test foot4 in leg 4 workspace 

footnum ~ 4; // select foot number four 

//test for intenection with outer workspace limit 

intercept_flag = aicint(dfoot, dir, cenb^ outrad, 

seg4nout, aeg4pout, footnum, inlercepts, mdistance); 

if (iidercept_flag " 1) 
distance_b ° mdistance; 

//test for intersection with inner workspace limit 
intercept_flag = arcint(dfoot, dir, cenb^ inrad, 

teg4nin, segdpin, footnum, intercepts, mdistance); 

if(intetcept_flag-=- 1) 
distance_b - mdistance; 

// test for intersection with CW workspace limit 
intercept.flag ” segint(dfoot, dir, teg4pin, seg4poiit, 

intercepts, mdisUnce); 

if (iotercept_flag == 1) 
distanGe_b • mdistance; 

//lest for intersection with CCW workspace limit 
imeicept_flag = segiiitfdfoot, dir, seg4nin, seg4nout, 

intercepts, mdistance); 

if (iiitereept_llag = 1) 
distance_b = mdisUmce; 

// test foot6 in leg 6 workspace 
footnum = 6; // select foot number six 
//test for intersection with outer work^Mce limit 
iiiterccpt_fUg = arcinl(ffoot, dir, cenbod, outrad, segdnout, seg6pout, 

footnum, intercepts, mdistance); 

if (intercept_flag = 1) 
distance_c = mdistance, 

//test fm intersection with inner workspace limit 
intercept_flag = arcintfffoot, dir, cenbe^ inrad, seg6nin, seg6pin, 

footnuiit, intercepts, mdistance); 

if (intercept_flag =1) 
distance_c = mdistance; 

// test for intersection with CW workspace limit 
iiitetcept_ilag = segint(£foot, dir, seg6pin, seg6pout, 

intercepts, mdistance); 

if (intercept_flag =1) 
distance_c = mdistance; 

// test for intersection with CCW workiqrace limit 
intercept_flag = segintfffoot, dir, s^nin, segfinout, 

intercepts, mdistance); 

if(intercept_flag== 1) 

(listance_c = mdistaiKe; 





maxdut > min(niiii(dMliiice_a. disUBce_b), distaoce_cX 
} // tUt OKb tripod 1 toriiiig 
return (imxdiit); 

} // thit ends fimction ouxdiftaiice_2}cDifoat 

//*«*M**..****««M*******>n>S*aiid«PHIU*****************************' 

//TITLE: inixilwt«nce_45cinfoot0 

/'L'lPUT: (x,y)body coordinates ofaUrixfMtmd body center, tripod 
'' munber (0 - tripod 0,2 - tripod 1); directico 

//OUTPUT: nuudmum stride possible without exceeding workspace 
//FUNCTION: This function finds the maxiniuin stride possible for AquaRobot, 


// using a tripod gait, given an arbitrary dire^on as an mpuL 

//..*«....******«««***..AQI;AR0B0T«************************< 


double nuxrtistance 43ctnfool(Ne)d_Motion ftam, WaDcParameter Ikwp) 

{ 


ini arcintfdouble*. double, double*, double, double*, double*, mt, double*, double A); 
iitt segiid(double*, double, double*, double*, double*, double A); 

// initialization 

double inrad • 43.0; // imier radius of workspace 

double outrad • 149.27; // outer radius of workspace 

double maxdist, distaiKe_a, di s tanoe_b, distance_c; 

double inlerccpts[2]; 

double diitatKy = 0; 

int itnetcept_iUg ~ 0; 

ini footmim; 


double afoot[3]; 

double bfool[3]; 

double cfoot(3I; 

double dfoot[3]; 

double efoot(3]; 

double ffoot[3]; 

double cenbod(fi]; 

afoot[3] = nm.foot_l_coord(3]; 

bfool(3] = niafoot_2_coord[3]; 

cfoot[3] = niafoot_3_eoord[3]; 

dfoot(3] = rBn.foot_4_ooord[31; 

efoot(3] = nnLfoot_3_coord(3]; 

fibot[3] = nitLfoot_6_coord(3]; 

cenbod[6] = nm.body_center_coord(6]; 

double dir = wp.direction; 

mt tnpod = wp.phase; 

static double seglpin[2] = {43.0, 0.0}; 

H inside endpoint of CW segment #1 
stauc double seglpout[2] = {139.0446, 34.2967); 
// outside endpoint of CW segment #1 
static double seglnin[2] = {43.0,0.0); 

// inside endpoint of CCW segment #l 

static double seglitout{2] »= {139.0446, -34.2967}; 

// outside endpoint of CCW segment #1 

static double seg2pin(2] = {22.3,38.9711}; 

// inside endpoint of CW segment #2 
static double seg^pout[2] = {22.3,147.3643}; 

// outside endpoint of CW segment #2 
static double seg2nin[2] = {22.3,38.9711}; 

// inside endpoint of CCW segment #2 

static double seg2iKiut[2] - {116.3446,93.2678}; 

// outside end^int of CCW segmeid #2 
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sUticdoublescg3pii>(2] ^ {-22.3,38.9711}; 

// inside endpoint of CW segment #3 
static double seg3pout{2] = {-116.5446,93.2678}; 
// outside endpoint of CW segment #3 
static double scg3nin[2] = {-22.5,38.9711}; 

// inside endpoint of CCW segment #3 
static double seg3nout[2] ~ {-22.3,147.3643}; 

// outside endpoint of CCW sepnent #3 


static double seg4pin(2] {-45.0,0.0}; 

// inside endpoint of CW se^nent #4 

static double se^pout[2] = {-139.0446, -54.2967}; 

// outside ent^toint of CW segment 44 
static double seg4nin(2] = (-43.0,0.0); 

// inside endpoiid of CCW segment #4 

static double seg4nout(2] = (-139.0446, 54.2967); 

//outside endpoint of CCW se0nent#4 

static double seg3pin[2] = (-22.5, -38.9711}; 

// inside endpoint ofCW segment #3 

static double seg3pout[2] ^ (-22.3, -147.3645}; 

// outside endpoint of CW sepnent #3 
static double seg5oin[2] > (-22.3, -38.9711}; 

// inside endpoint of CCW segment #3 

static double seg5nout[2) = {-116.5446,-93.2678); 

// outside endpoint of CCW segment 43 

static double seg6pin[2] = {22.3, -38.9711}; 

// inside enc^int of CW segment 46 

static double seg6pout(2] = (116.5446, -93.2678); 

// outside endpoint of CW segment 46 
static double seg6oin[2] = (22.5, -38.9711}; 

// inside endpoint of ^W segment 46 
static double seg6nout[2] = (22.3, -147.3645); 

// outside endpoint of CCW se^nent 46 

if (tripod = 0){ 

// test footl in leg 1 workspace 

footnum = 1; // select foot number one 

// test for intersection with outer workspace limit 

interceptjlag “ areintfafoot, dir, cenbod, outrad, seglnout, seglpout, footnum, intercepts, distance); 
if (intercept_flag =» 1) 
distance_a distance; 

// test for intersection with CW workspace limit 
intercept_flag = segint(afoo(, dir, seglpin, seglpout, intercepts, distance); 
if (intercept_flag =1) 
distance_a = distance; 

//test for intersection with CCW workspace limit 
intercept_flsg = segint(afoot, dir, seglnin, seglnout, intercepts, distance); 
if(intercept_flag“ 1) 
distance_a ° distance; 

// test foot3 in leg 3 workspace 
footnum = 3 ; // select foot number three 
// test for intersection with outer workspace limit 
intercept_flag = arcinlfcfoot, dir, cenbod, outrad, 

seg3nout, seg3pout, footnum, intercepts, distance); 

if (intercept_fUg = 1) 
distance_b = distaiKe; 

//testfor intersection with CW workspace limit 
intercept_flag = segintfefoot, dir, seg3pin, seg3pout. 
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imercepts, distance); 

if (iineTce{il_flag — 1) 
disUoce^b ** distanoe; 

//test for intersectiaa with CCW woric^Mwe limit 

iidereept_flag > segintfGfoot, dir, seg3nin, segSnout, 

intercepts, distance); 

if(intereept_fUg“= 1) 
distance_b = distance; 

//test foots in leg S workspace 
footnum S; // select foot number five 

//test for intersection with outer workspace limit 
interGept_flag = arcintfefoot, dir, cenbod, outrad, 

segSnout, segSpout, footnum, intercepts, distance); 

if(intercept_flag= 1) 
distance_c > distance; 

//test for intersectioo with CW workspace limit 
intercept_flag = segintfefoot, dir, segSpin, segSpout, 

intercepts, distance); 

if(intercept_flag= 1) 
distanoe_c =* distance; 

// test for intersection with CCW workspace limit 
intercept_flag = segintfefoot, dir, segSnin, segSnout, 

intercepts, distance); 

if (iiitercept_ilag =1) 
distance_c = distance; 

} //this ends tripod 0 testing 

else if(lripod = 2){ 

//lest foot2 in leg 2 workspace 

footnum “ 2; // select foot number two 

//test for intersection with outer workspace limit 

intercept_flag = arcintfbfoot, dir, cenbod, outrad, 

seg2nout, seg2pout, footnum, intercepts, distance); 

if (intarcept_flag =1) 
iistance_a = distance; 

//test for intersection with CW workspace limit 
Litercept_flag = segintfbfoot, dir, se^pin, seg2pout, 

intercepts, distance); 

if (inteTcept_flag =1) 
distance_a = distance; 

// test for intersection with CCW workspace limit 
iirtercept_flag = segintfbfoot, dir, s^nin, $eg2nout, 

iirtercepts, distance); 

if (interceptjlag = 1) 
distatKe_a = disUttce; 

//test foot4 in leg 4 workplace 
footnum = 4; // select foot number four 
//test for iirtersection with outer workspace linrit 
interceptjflag = arciirtfdfoot, dir, cenbod, outrad. 

seg4nout, seg4pout, footnum, intercepts, distance); 

if (intereept_flag 1) 
distaftce_b = distance; 

// test for intersection with CW workspace limit 
iirterceptilag = segintfdfoot, dir, seg4pin, seg4pout, 

irrtercepts, distance); 

if (intercept_flag = 1) 
distance_b = distance; 
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// test for intenection with CCW workspace limit 
iiiteroept_flag =■ segint(dfoot, dir, segdnin, seg^Kxit, 

intercepts, distance); 

if (intereept_flag == 1) 
distance_b > distance; 


//test fi)otd in teg 6 woikq>ace 
fixitnum 6;//select foot number six 
//test fix intersection with outer workqfMce limit 

inten;q)t_flag = arcint(ffoot, dir, cenbod, outiad, segdnout, seg6pout, 
footnum, intercepts, distance); 

if (intercept_flag = 1) 
distanoe_c ° distance; 


//test for intersection with CW workspace limit 
inteicept_flag = segintfffoot, dir, segdpin, segdpout, 

intercepts, distance); 

if(intercept_flag= 1) 
distaiice_c = distance; 

// test for intersection with CCW workspace limit 
intercept^flag = segint(£foot, dir, seg6nin, segdtMut, 

intercepts, distance); 

if (intercept_f]ag =1) 
distaiKe_c > distance; 


} //this ends tripod 1 testing 

msvdict = min(min(distance_a, distance_b), distance_c); 


return (maxdist); 

} // this ends fiinction iruxdisUnce_4Scmfoot 

//.*«**.*.*«**«*..««»«*»;VPS*aiid*PHRI********************************* 

//TITLE; arcintO 

//INPUT: (x,y)body coordinates ofa selected foot; direction; bocW center, 

// radius ofworkspace; CCW arc segment en(^int$;CW arc segment 

// endpoints; foot number selected 

//OUTPUT: intescepts of arc segment; mdistance to the intercepts fiom the 
// current foot position; flag to indicate whether an intercept 

// was found 

//FUNCTION; This function determines whether there is an intercept of the 
// desired direction ofthe foot with an arc segment defining a part 

// of the workspace. 

//«»•••••••«»••••••«»»»» AQUAROBOT*********************************** 

int arcintfdouble footQ, double direction, double cenbod[], double radius, double negJimitQ, double pos_limit[], int foot_number, 
double interceptsQ, double Amdistance) 

{ 

//initialization 

double a, b, c, d, x_intetcept, y_intetcept; 
double perp_di^ance, len^ x_petp, y_petp; 
double rad_$({uared, perp_squared; 
double radius_test, in_direction, out_direction; 
double atc_fi)ot, neg_arc_limit, pos_atc_Iimit; 
int arcsegment_flag = 0; 


peip_distance = ((cenbod[4] - foot[s])*cos(direction)) 
- ((cenbod(3] - foot[0])»siri(direction)); 
(perp_distance <= radius){ 
radsquared = radius*radius; 
perp_squared = perp_distaiKe*peip_distance; 
if (p^_distance < 0) 

length = sqrt (-(rad_squared - perp_squared)); 
else 
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Icogih ■> sqft(r»l_sqtured - pap_squared); 

//find (x,y) at intenectioo of peiptndicuUr mdisUnce and length 
x_pcfp = oenbod{3] + (pap_diitance*sin(directioD)); 
y_peip = cenbod[4] - (peq>_distance*oiM(<iirectioa)X 

//test if foot is inside or outside radius 
a » (foot(l] - oenbod{4])*(foat[l] - cenbod{4]); 
b « (fi>ot{0] - oeidMd[3])*(fi>ot[0] • codKMip]); 
radius_test» sqrt(a b); 

if (radius_test > iadius){ 

// find (x,y) at intersection of ray and arc if foot is outside radius 
x_inletccpt > x_pen> - (Ic<igth*oos(diieclion)X 
y^intercept = y_petp - Oc)>glh*sin(directian)X 
iiiteroepts[0] - x^inlercept; 
interoqits{lj ° y_intercc{it; 

> 

else { 

//find (x,y) intersection of ray and arc if fi>ot is inside radius 
x_iiderccpt >= x_perp + (lenglh*oos(direction)); 
y_ifitercept = yj>*rp + Oenglh*$in(direction)); 
intetoe|its[0] = x^intercept; 
interce|)ts(l] = y_intetcept; 

} //radius test ends here 

//test if intersection is in desired direction 

in_direction = fi>ot[0]*oos(diiectian) foot[l]*sin(direction); 

out_directioo * x_iiitercept*cos(directioo) y_intetcetit*sin(directionX 

if (in_diiection <= out_direction){ 
c * ^_intetoept - foot[l])*(y_interoeiit - foot[l]); 
d = (x_intercept - foot{0])*(x_iiiterce|it - fix>t[0]); 
mdistance = s^c (Q; 


//test if intersection it Hfitfain arc segment 
arc_fi)ot = atan2(y_intercept • cenbod[4], x_interce]X - cenbod[3]X 
iieg_arc_liniit ataii2(neg_liinit[l] • oeiibod(4], neg_liiiut[0] • cenbod[3]}; 
pos_arc_Uniit = ataii2(pos_liinit[l] - cenbod[4], po$_limit[0] - cenbod(3]); 

//tests to ensure proper sections of arcs are used 
if (foot_ouinber = 1){ 

if (arc_fi>ot >= neg_arc_Iimit arc_fbot <= pos_arc_Ii 2 nit) 

arcsegaieiit_fUg = 1; 

} 

else { 

if (direction > 0){ 
if (neg_arc_liinit < 0) 
iKg;_atc_liitiit “ neg_arc_limit + (2*PI); 
if (pos_arc_liinit < 0) 
pos_atc_limit = pos_arc_liiiiit + (2*PI); 
if (aic_foot < 0) 
arc_foot = atc_foot + (2*PI); 

if (arc_foot >= neg_arc_limit && arc_foot <= pos_arc_timit) 
arcsegnient_fiag = 1;, 

} 

else { // direction < 0 
if (neg;_arc_limit > 0) 
neg_arcjimit = neg_arc_limit - (2*PI); 
if (pos_arc_liiiiit > 0) 
pos_arc_liinit = pos_arc_iiinit - (2*PI); 
if (arc_foot > 0) 
arc_foot = arc_foot - (2*PI); 

if (arc_fi>ot >= neg_atc_limit &&. arc_foot <= pos_arc_liinit) 
arcsegnMiit_flag = 1; 

} //direction test ends here 
) //foot nuinber test eixls here 
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} //inicnection tot cads here 
}//perpindicuUr test ends here 


if(arcsegiiient_iUg != 1){ 
arGsegmeat_fUg - 0; 

} // nullifying panunelers ends here 

return (srcsegnient_flsg); 

} //arcintfimctian ends here 

//.*.*****««.*«*«***«***««^*^*PHSi***«»«««***<***«***«*M**< 

//REVISED: . 

//TITLE: segintO 

//INPITT: (x,y)body coordinates of a selected foot; direction; inner 
// endpoints; outer endpoints 

//OUTPUT: intercepts of line segment; mdistance to the intercepts from the 
// current foot position; flag to indicate whether an intercept 
// was found 

//FUKCnON: This function determines whether there is an intercept of the 
// desired direction of the foot with s.'-<se segment defining a part 

// ofthe woflcqiace. 

//.,.*«».....««.«.«..««^qUaroBOT********************‘*«****** 

int segint(doubte firotQ, double direction, double insegQ, double outsegQ, 
double interceptsQ, double &indistance) 

{ 

//initialization 

double a, b, c, d, theta_segment, theta_di£ferenoe; 
double x_oumetator, y_nunierator, denominator, 
double x_inletcept, y_intetoept; 
double segment_test, CCW, CW, point_l, point_2; 
iid linesegment_ilag 0; 


//find theta of line segment 

tbeta_segtnent ^ atan2(outseg{l] • insegfl], oatseg[0] - inseglO]); 
tbsta_difrerence direction • theta_segment; 


// if there is no difference between direGtiao ray and the orientation of 
//the line segment being tested, then they ate parallel; no intersection 
if( tbeta_diffeteace != 0.0 
AA tbeta_difretence != PI 
AA theta_difrerence !> -PI){ 

b - (inseg{0]*sin(theta_segmeiit)) • (itiseg[l]*cos(theta_segment}); 

a > (foot[0]*sin(ditection)) - (fo^l]*oos(direction)); 

x_numetator = (•oos(theta_segment)*a) (cos(direction)*b); 

y_iiuineiator » (sin(direction)*b) - ($in(theta_segment)*a); 

denominator = riii(theta_segment - direction); 

x_in^xcept = xjnumerator / denominator, 

y_intercept ^ y^numerator / denominator, 

intercepts[0] == x_intercept; 

intercepts[l] = y_interGept; 


// determine if the direction ray intersects the line segment 

CCW = (inseg[0]*cos(theta_segment) + inseg[l]*sin(theta_segniem)); 

CW = (outseg(0]*cos(thela_segment) + outseg(ll*sin(theta_segment)); 
segment_test (x_intercept*cos(theta_segment) 

y_intercept*sin(theta_segment)); 

if ((CCW <= segment_test) AA (CW >= segmeot_test)){ // if true, intersection 
// determine if the orientation is correct 

point_2 = (x_intercept*cos(direction) + y_intercept*sin(direction)); 
point_l = (foot[0]*c^direction) + foot[l]*sin(direction))'. 

if (point_2 > point_l){ 

// correct orientation 
linesegment_flag = 1; 


195 












c • (y_inicroept - fooi(l])*(y_inianpl - foot{l]); 
d > (x_ioleroept - foal{0])*(x_inlerocpl - foot(0]); 
mdktancif - s^c + dy, 

} //orienutioa tot eodi here 

}//line segment inlenection test endi here 

} // line intenectioateM ends here 

//if one of the tests &ib, there is no nXenectioa 
if (l iuuepn ent_fl«g!- 1){ 
linesegment_fUg 0; 

} //nullifying panmeten ends here 

return (linesegnient_flag); 

} II segint fiiitctioa ends here 

#ifiidefARn;NC_H 

MefineARFUNC.H 

// FILENAME: srfiinc.H 
//PURPOSE. 

// CXtMMENT: include file 

#include 'atcoosLH* 

#include 'Kineinstics.H' 

extern 

double min(double, double); 
extern 

double nux(double, double); 
extern 

void ellipte(WelkP«rameter &, double *, double *); 
extern 

void kinematicsfdouble*, double*, double*, double*, double*); 


extern 

voidim' ' inemstict(double*, double*); 


extern 

void world_body(doubie*, double*, double*); 
extern 

void body_warld(doubte*, double*, double*); 
extern 

int srcint(double*. double, double*, double, double*. double*,iM,double*, double &); 
extern 

int segiin(double*, double, double*, double*, double*, double &, int); 
extern 

double maxdistance_2Scinfoot(Next_Motioo &, WalkParameler St), 
extern 

double nuxdistence_4Scinfoot(Nexl_Motion St, WaOcParanieter ft); 

#endif 

// FILENAME: artpgsitC 
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//PURPOSE: tripod gut using ellipte trajectory 

//COMMENT: to mske know the motor speed. 


^include <stdio.h> 

#inciude <slreanUi> 

^include <math.h> 

Mnchide <stdlibJi> 

^include 'ercoostH* 
iiinclude '«iiuiic.H* 

#inchide ’ertpgait.H' 

#iiiclude *Kineniatics.H* 

// FUNCTION: set_goalO 

void get_goel(douUe goslQ) 

{ 

printfl^Goal point( x y )==>"); 
scwfC’ftif Kir, AgoalpCW], &goel[YW]); 

}// end of get_goalO 

II FUNCTION: get_aienuO 

int get_nienu(int menu) 

{ 

printfr'O...Discrete Motionin'*); 
printI('l"-C<totinuous Motion ■=>*); 
scuifi7%d", Amenu); 


letum (menu); 

}// end of gct_tneaaQ 

//FUNCTION: get footchoiccQ (uUed 06May93) 

int get_footcfaoicc(iDt footsize) 

{ 

printf(''0...23 cm dismeter fbotpad\n"); 
printf(*1...4S cm diuneter footpad “=>"); 
scanflT^iir, Afoolsize); 

return (footsize); 

}// end of get_footchoiceO 

... 

// FUNCTION: gait_algotittaoO 

void gait_algorifhm(Nexl_Motion Aactual, 
double goalQ, 
int menu, 

int footsize) // selecting discrete or continuous motion 

{ 

static WalkParameter wp; 
static Flag flag; 
static int counter. 


switch (menu) { 
caseO: //discrete motion 
/•set flag*/ 

diac_set_flag(actual, wp, goal, flag); 
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/* gAltjtiUBUIlg */ 
if (flag.phtie£ad ** 1) { 

disc_giit_pbiaiiii8(«ctual. wp, Oag, goal, fooliize); 

> 

/* Motion Ptaoning */ 
diicJripod_matiaa(actiul, wp, flag); 

bteak; 

easel: //conliiaiousmotion 
/•set flag*/ 

cont_set _fl a g (a rti ial, wp, goal, flag); 

/* gait_planoing */ 
if (fUgphaseEod *=* 1) { 
oont,gait_planning(aonial, wp, flag, goal); 

} 

/* Motion Planning */ 
coiit_tripod_motion(actual, wp, flag); 

break; 

de&utt 

break; 

) 

// ptiitt_wa]kpafa(wp); 

}// End of gait_algonthmO 

// FUNCTION: robot_modeiO 

void robot model(Next Motion ftnm) 

{ 

double euler(3]; // Euler angle 

double bodypj; // Body Position 

double jangle[L£G](JOINT]; //Joint angle 
double jpotnt^XG][JOINT]pCYZ]; // Joint Position 
int leg, joint, axis; 


/* Initialize */ 

eulerfO] - nRLbody_oenter_coord[0]; 
euler{l] - inLbody_ccnler_ooard[lj; 
euler(2 j«ini.body_cenleT_oootd(2]; 
bodyfxW] - nni.body_center_cootdp]; 
bodyfYW] * nmbody_oenter_coord[4]; 
bodyjZW]» noLbody_center_coord[S]; 

/* joint angle initialize */ 
for (leg>LEGl; leg<=LEGd; leg^^) { 
for(joint”CB;joint<“FOOT;joint-t-+) { 
jangle(leg][joint] - nm.iid>dJoint angle(leg][joint]; 

//Tricky part! 5ibdJoiiit_angleQ[HIP->KNEE2](HlP,KNl,KN2,FT} 
//Tricky part! janglea[CB->KNEE2){CB,HIP,KNl,KN2,FT) 

} 

> 

for Oeg=LEGl; leg<-LEG«; leg^-t-) { 

/* Kinematics computation for GNU plot data file */ 
kineniatics(jangle(leg], jpointneglpriP), jpoiiit[leg](KNEEl], 

jpointileg] [KNEE2 J, jpoint(leg] [FOOT]); 
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for(ioint=°HIP; join»<*FOOT; joiHt++){ 

/* Coordniale TnnsfannitionCWORLXK-BOOY) */ 
warid_body<eiiler, body, jpoint[le^[joint]); 

} 

) 

/* set Next_Mo(ioii •/ 

for (axis=XW; •xis<»ZW; «xi*++) { 

aiafoot_l_coord{axis] ~ jpoiiit[LEGijiFOOT](udt]; 
iiin.foot_2_coatd{axu] - jpoiiit(L£G2j[FOOT](axisj; 
nmjfoot_3_coord[axisj» jpouit{LEG3j[FOOT][axisj; 
iiiiiJbot_4_coocd[axisj - jpoiiitlL£G4j(FOOT)(axis]; 
iim.foot_3_ooord[axis]» jpoiiit[LEG3j(FOOT][axaj; 
niiLfoot_6_coord{axis] ~ jpoint[L£G^j[FOOT][axit]; 

> 

/* set contact fUg.*/ 
if (niiUbot_l_coord[ZW] >- -0.01) { 

nm.leg;_coiitact_iUg(L£Gl] • 1; // set contact flag(Legl) 
am-foot l_coord(ZW] 0.0; 

} 

else { 

nm.Ieg;_coatact flag{L£Gl] = 0; // reset contact flag 

} 

if (nm.foot_2_coord[ZW] >- -0.01) { 
iim.leg_oontact_flag(LEG2] > 1; // set contact flag(Leg^) 
nmibot 2_eoofd(ZW) “ 0.0; 

} 

else { 

nni.leg_contact fIag(LEG2] > 0; // reset contact flag 

} 

if (nmibot_3_coord[ZW] >- -0.01) { 

nm.leg_oantact flag[LEG3] =■ 1; // set contact flag(Leg3) 
miLfoot 3 coocd[ZW]»''0.0; 

} 

else { 

nm.leg_contact flag[LEG3] •• 0; // reset contact flag 

} 

if (nnvfoot_4_coord[ZW] >= -0.01) { 
nnvleg_cootact_fla^LEG4] = 1; // set contact flag(Leg4) 

} 

else { 

nm.leg_contact flag[LEG4] = 0; // reset contact flag 

> 

if (nm.foot_5_coord(ZW] >= -0.01) { 

nm.leg_cootact_flag[LEGS] = 1; // set contact flag(LegS) 
nni.foot_5_coord[ZW] =7).0; 

} 

else ( 

nm.leg_contact_flag[LEG3] = 0; // reset contact flag 

> 

if (nm.foot_6_coord[ZW] >= -0.01) { 
nm.leg_contact_flag[LEG6] = 1; // set contact fIag(Leg6) 
nm.foot_6_coord[ZW] = 0.0; 

} 

else { 

nm.leg_contacl_flag|LEG6] = 0; // reset contact flag 

} 

)// End of robot_niodel0 
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//TITLE: diic_fet_£UgO; 

//Fl/NCnON: taiiOagt. 

// INPUT: phaie, Next_Motiao cUm, Flag class. 

//OUTPUT: Flag dan 
// CALLED BY: gait_algaritlanO 
//CALLS: Nooe 
//COMMENT: fiirdiscretemotioo. 

H *••••*•*••••••••••••••*••«•***«•««««••«•««*•«*«••••*••••••••«••**•••*« 

void disc_set_flag(Next_Motioii &an, 

WaOcParanMlCT &wp, 
double goalQ, 
FlagAfiag) 

{ 

double distance; 

/* set phase end flag */ 
if((wp.phase"0)&& //TPOphase 

(nm.leg;_coiitact_flag[LEGl] ^ 1) && // TPO cootuct 
(flag-p h as e E n d —° 0)) { 
flag.phaaeEnd * 1; 

} 

elseif((wp.phase“2)&A //TPl phase 

(iiin.leg_oocitact_flag[LEG4] 1)&&//TPl contuct 
(flag.phaseEnd -= 0)) { 
flag.phaseEnd ° 1; 

} 

else if ((wp.phase BODYO) && // BODY phase 

(fibs(nm.body_cenler_ooord[3] - (nm.foot_l coordpC] - TripodSize)) 

< 1 . 0 )&& 

(flagphaseEnd = 0) ) { 

flag.phaseEnd -1; 

} 

else if ((v»si.phase " BODYl) &&. // BODY phase 

(ftbs(imi.body center co^P] • (miLfi>ot 4 coord[X] TripodSize)) 

< 1 . 0 )&& 

(flagphaseEnd = <>)){ 

flag.phaseEnd • 1; 

} 

else{ 

flag.phaseEnd = 0; 

} 

/•Set GOAL flag*/ 

distance = sqtt( (nin.body_ceater_ooord[3] • goaipiW]) 

•(nm.body_cenler_cootd(3] - goaipCW]) 

+ (iiin.bo^_center_ooord[4] - goalfYWJ) 
•(nm.body_center_oootd[4] - goalfYW]) y, 

if (distance <« wp.bodyspeed) { 
flag.phaseEnd 1; 
flag.goal = 1; 

} 

}// End of disc_set_flagO 

//•.•*****.••*•**••••••••••*•*••*•*••••••••«•••***•***•*•*•**••••**•*•>* 

//TITLE: disc_gait_planningO 

// FUNCTION: Gait planning This fimction determins walking parameters. 

// INPUT: motion phase, WalkParameter class, Flag class. 

// OUTPUT: WalkParameter class. 

//RETURN: motion phase. 

// CALLED BY: mainO- 
// CAIXS: None 
//COMMENT: for discrete motion 

//*«•**••*•*•«••*•••*••••**«••*••**••••**•**••••*•******••••*•••*•**••• 

void disc_gait_planning(Next_Motion &nm, // 

WalkParameter Awp// 
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FUg&flaji, // 
double goaipOQ, 
int footsize) // g(^ poeitioo 


{ 

static int counter = 0;//to count phase number 

double distance; // distance between START and GOAL 


. goal case •/ 
if(flag.goal = l){ 
exit(0); 

} 

/* to find direction */ 

wp.direction * atan2(goal(YW] - nnLbody_ceiiter_coord[4], 

goal|XW] • imi.b(^_center_coanl[3]y(DR); // in degrees 

/* to determine a stride */ 
if (couitter = 0) { 

printlf*\n"); 

printirPHASEO = 44dto“, wp.phase); 
if (footsize = 0) { 

wp.stri^ = maxdistance_2}cmfoot(nm, wpX 

} 

else { 

wp.stride = maxdistance_4Scinfoot(nm, wp); 

} 

} 

eise{ 

/* increment phase */ 
wp.phase wp.|duse + 1; 
wp.phase => wp.phase % 4; 
pin^THASE = %dto". wpphase); 

if(footsize = 0) { 

■f ( (wp.phase “ 0) 8 (wp.phase = 2) ) { 
wp.stride >= maxdikance 25cmfoot(nra, wp); 

- } 

else { 

if (counter == 1) { 

wp.bodyspeed O.S; 

} 

else ( 

wp.bodyspeed = 0.3; 

) 

} 

> 

else { 

//this area is for 43cm fyotpad, duplicate 23cm footpad routines 

} 

} 

eounter++; 

/* to find distance between ament position and GOAL */ 
distance = sqtt( (goal[XW] - nm.body_center_ooord[3]) 

*(goaipCW] - nm.body_center_coord[3]) 

+ (goal[YW] - nm.body_center_coord[4]) 

*(goal[YW] - nm.body_center_eoord[4])); 

/* handlingincaseofrobot is near the goal */ 
if (distance < (wp.stride/2.0)) { 
wp.stride = 2.0*distance; 

} 
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}/* Ei^ of disc_g*it_pluniDgO 


// FUNCTION: ^sc ti9od_iD0liaa0 
//PURPOSE: discr^tripodmoliaa. 

// to compute one in ct ci nsntel motioa ustng ellipK tnjcctoty. 

// to calculate one inameoied joint statuses 
// INPUT: class Nexl_Motioii, 

//OUTPUT: classNext_Motioo 
//COMMENT: called by oiock»_plaiaiai80- 

void disc_tripod_inotiaa(Next_Motiaii Amt, 

WalkPsranMter Awp, 

FlagAflag) 

double eulesflbreeDim), oldeolertThreeDim]^/ Euler angles 

double deulerTTIireeOim]; //Chai^ in Euler an^e 

double bodyfTlaeeDim], oUbodyflbreeDim]; // CB positioD on World 

double dbodyfThreeDim]; // Chai^ in BODY position 

doubleoliya^[L£G][K)INTI; //oldjoint angles 

double jang|e[LEG][JOINT]; //joim angles 

double jpoii)t[L£G](X>INT](ThreeDiffl]; //joint positioiis 

static double foolptiBt(LEG] [TbreeDiffl] ; //foot priit(cootacte<i point) 

double dbm; 

int le& joint, axis; 

/* Initialization */ 

for (axis«XW; axis<“ZW; axis++) { 

/*euler angles are not needed for ver. 1.0 */ 

euler[axis] “ oldeulerjaxis] = nm.body_oeoter_coofd(axis]; 

bodyfaxis] ~ otdbody(axis] = nm.body_ceiiter_coofd(axis+3J; 

/* Feet position initialize */ 

jpointILEGl](FOOT][axis] • nm.foot_l_coordlaxis); 
jpoint[LEG2][FOOTl[axis]» nBLfi>ot_2_coord(axis]; 
jpoint[LEG3][FOOT}ltxit} -nniJbot_3_coord(axis]; 
jpoint[LEG4j[FOOT)[axisi => nnLfoot_4_coord(axis]; 
jpoiiit[LEG5](FOOTl(axit] = nm.foot_5_coord{axis]; 
jpoint[LEG«][FOOT]iaxis] • mn.foot_6_coord{axisj; 

/* footprint initialize */ 
if (flag.pfaaseEnd 1) { 

if InnLleg contact flagfLEGlI "1) 
footpriiil(LEGl][axis] > onLfoot_l_coo(d{axis]. 
if (iwileg_contact_flag(LEG2] = 1) 
footpriiit(L£G2][axis] - nm.foot_2_eoord[axis]; 
if(nm.leg_oontact_flag(LEG3] = 1) 
footprint[LEG3][axis] = nmJbot_3_coord[axis); 
if (nm.leg_contact_flag(LEG4] = 1) 
footpriiit{L£G4][axis] - nmibot_4_ooord(axis]; 
if (nni.leg_eoiitact_flaglLEG5J “ 1) 
footprint[LEGS][axis] - nm.foot_5_coord[axisj; 
if (imleg_contact_flag(LEG6] ”= 1) 
footpriilt[L£G6](axis] = nm.foot_6_coord[axis]; 

} 

> 

/* Joint angle initialize */ 
for (leg=LEGl; leg<=LEG6; legt-t-) { 
for (joint=CB; jonrt<=FOOT; joint++) { 
jangle(leg][joint] = nnLinbdJoint_angle[leg][joint]; 
oldjangle{leg]{joint] = jangle{leg][/oint]; 

//Tricky part! inbd_joint_angleQ[HIP->KNEE2){HlP,KNl,KN2} 
//Tricky part! jangleG[CB->KNEE2){CB,HIP,KNl,KN2> 

} 

> 
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if ((wp.piuict€2) ^ 1) {// if BODY pinie, tiien increment body poeition 
dfam = wp.bodyspced; 


/* to incnment body poatico */ 
bodypCW] <l)m*ooa(wp.direcliao*DR); 
bodyfYW] ■*= dbm*sin(wp.diRctkn*DR)i 
bo<^lZW]+=0.0; 

} 

/* to calculile joint angla */ 

for (Ieg=LEGl; leg<=LEG<; lef*-+) { 

if( ((wp.phase ^ 0) ((teg%2) = 0)) //tpO AND odd leg# 

i((wp.ph»*e“ 2) &&((le^/42)-“!))){ //tpl AND even leg# 

/* compute FOOT potitioa on <n ellipce curve respect to WORLD */ 
ellipsefwp, footprintfleg], jpoint[leg](FOOT]X 

/* Foot CcoUct Conditioa*/ 
if (ipoii#(leg|[FOOT](ZW] > 0.0) ( 
jpointtlegJjPOOTJpfW] =• idotpriitpeg][XW] 

+ vvpjtTidc*cai(wp.direclion*DRj, 
jpoii«tneg][FOOT][YW] - footprii#{leg][YW] 
wp.stride*sin(wp.direction*DR); 
jpoint(teg][FOOT)(ZW]-footprintpeg][ZW]; //O.O; 

> 

} 

/* Coordinites Tnnsfonn>tioa(BODY<-WORLD) •/ 
body_warld(euler, body, jpointneg][FOOT]); 

/* Inverse Kinematics */ 

inv kineinatics(jpoint[leg][FOOT], jangle(teg]); 

} 

/* set joint angle */ 

foi(joint-CB; joint<*KNEE2; jomt++){ 

fof(leg-LEGl; leg<-LEG<; legv*){ 
niainbdjoint anglePeg][joint] ~jangle[leg][ioinl]; 

//Tridcy part! ^Joint angleQ(HIP->KN£E2](HIP.KNI,KN2,FT} 

//Tricky part! jangieO[CB->KNEE2]{CB,HIPJ<NlJCN2,FT} 

} 

} 

/* to set next status */ 
nin.body_center_CDord(3] = bodypCW]; 
nm.body_center_coord(4] • bodyfYW]; 
nm.body_certer_coord[5 j = bodyfZW]; 

/* to calculate duuige in body position and euler angles */ 
for (axis=XW; axis<=ZW; axis-H-) { 

deulerfaxis] = eulerfaxis] - oldeulerfaxis]; 
dbodyfaxis] = bodyfaxis] - oldbodyfaxisj; 

} 

/* to calculate diange in BODY posHioo */ 
nm.bodyniotioa[0] > deulerf AZIMUTH]; 
nm.bodymotion[l] = deulerfELEVATION]; 
iinLbodymotioo[2] = deulerfROLL]; 
nm.bodyniotion[3] dbodypCW]; 
nm.bodyinotion[4] = dbodyfYW]; 
nm.bodynK>tion[S] = dbodyfZW]; 


/* to calculate change in joint angles */ 
foffjoint=HIP;joint<=KNEE2;joint++){ 

iim.leglinotion[joiiit-l] >jangle[LEGl][joint] -oldjangle[LEGl][joint]; 
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Bm.leg2inoli<iii[>oiiit-1] - jaiigle[L£C2][|oint] - old)aogle(L£G2][jou]t]; 
i)m.leg3matioii{io^-lj > j«iigle[LEG3j[)oiiili - oUyaiig|e(L£G3][]oint]; 
nm.leg4iiiolioa[joilat-l] ~ jaiiglc[L£G4][joint] - oldjingle[LEG4j[joint]; 
nalcg5ino(iaa(ioint-lj > jangkpjEGsjyoint] - ol(4ai>gtc(LEGS][joint]; 
l■n.leg6lDo(ioo[)oint-l] =■ jaiiglc(LEG6]£joint] - okyan^(L£G6][jomt]; 

} 

}/* End of duc_tr^>od_mo(ioo */ 

//TITLE: coDt_tct_flaeO; 

//FUNCTION: tosctlUg*. 

//INPUT: phase, Nexl_Mo(ioa cUa, Flag clast. 

//OUTPUT: FUg class 
// CALLED BY: gait_algaritlmO 
//CALLS: None 

//COMMENT: forcoidinuousmotiflo. 

void coat_set_flag(Next_Motian Am, 

WalkParameler Awp, 
double goal(]. 

Flag&flag) 

{ 

double distance; 

/* set phase end flag */ 

if ((wp.phase 0) AA // TPO phase 

(nfn.le^ooatact_flaig{LEGl] "1) AA // TPO coiduct 
(flag.phaseEiid — 0)) { 
flag.ph^End - 1; 

} 

else if ((wp.phase *** 1) AA // TPl phase 

(nm.leg_contact_flag(LEG4] 1) AA // TPl contuct 

(flag.pltateEnd —> 0)) { 
flag.phaseEiid >• 1; 

) 

else ( 

fla^phaseEnd 0; 

} 

/•Set GOAL flag*/ 

distance ~ sqrt( (nni.body_ccater_coacd(3] • goaipCW]) 

*(nm.body_oenter_coord[3} - goaipcW]) 

+ (inLbody_oenter_coord[4] • goal[YW]) 
•(nri.body_center_coofdl4] - goai[YW]) y, 

if (distance <- wp.bodyspeed) { 
flag.phaseEnd - 1; 
flag.goal =* 1; 

} 

}// End of set_flag0 
// TITLE: gait_planningO 

// FUNCTION: Gait planning ntodule. This function determins walking panmeten. 
//INPUT: motion phase, WaDcParaineter class. Flag class. 

//OUTPUT: WaUcParameter class 
// RETURN: motion phase. 

//CALLED BY: mainO. 

//CALLS: None 

//COMMENT: for continuous motion 

void cont_gait_planning(Nexl_Motion Anm, // 

WalkParameter ScwpM 

Flag Aflag, // 

double goaipCY]) //goal position 


204 







{ 

static iot couider^; // to cotmt phase number 

double distance; // distance between START and GOAL 

/* goal case */ 
if (flag.goal >==• 1) { 
exit(0); 

} 

/* to detennioe a stride */ 
if (counter “ 0) ( 
wp.stride DdaultSTRIDE/2.0; 

wp.bodyspeed-((De£uiltSTRIDE/2.0yFINEy2.0; 

} 

else { 

wp.stride = DcfauHSTRIDE; 
wp.bodyspecd=(DefaultSTKiOE/2.0yFINE; 

/* increment phase */ 
wp.phase > wp.phase 1; 

wp.phase wp.phase % 2; // continuous:2, discrete:4 

counter++; 

/* to find distance between curroit positian and GOAL */ 
distance > sqtt( (goaI[XW] - nm.body_center_coord[3]) 

*(go^pCW] - nm.body_centsr_soard[3]) 

(goal[VW] - anLbody_center_coord[4]) 

•(goal[YW] - nin.body_center_coord(4])); 

/* handling in case of robot is near the goal */ 
if (distance < (wpjtiide/2.0)) { 
wp.stride ~ 2.0*distance; 

} 

/• to find direction */ 

wp.direction = atan2(goal[YW] - nm.body_center_coard[4], 

goaipCW] - nm.body_cenler_coord[3)y(DR); // degree 

}/* End of gait_plannmgO */ 

// FUNCTION; cont_tripod_mationO 
//PURPOSE: continuous tripod motioa 

// to compute one incranental motion using ellipse trajectory. 

// to calculate one incremented joiid statuses 

// INPUT; class Next_Motion, 

// OUTPUT; class Next_Motion 
//COMMENT: called by gait_planningO' 

void cont_tripod_nK)tion(Ne>!t_Motion &nm, 

WalkParameter &wp, 

Flag&flag) 

{ 

double euler(ThreeDim]. oldeulerfThreeDim],// Euler angles 

double deulerfThreeDim]; // Change in Euler angle 

double bodyflhreeDim], oldbody[ThreeOim]; // CB position on World 

double dbodyfThreeDim]; // Change in BODY position 

double oIdjangle(LEG][JOINT]; //old joint angles 

double janglc[LEG][fOINT]; // joint angles 

double jpoint{LEG] [X)INT] [ThreeDim]; // joitd positions 

static dcuble foo4)ritit[LEG][ThreeDim]; //foot print(contacted point) 

double dbm; 

int le£ joint, axis; 

/• Initialize •/ 

for (axis*XW; axis<=ZW; axis-M-) { 
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/* oiler eagles ere oot needed for ver. 1.0 

oilerfexis] • oIdeuler{axis] - noi.body_caiter_ooord(e»s]; 

body[exis] - ol(B>ady[exis] >iim.bo^_oenter_coatd[exis't-3]; 

/* Feet position initialize */ 
jlioii 4 LEGl][FOOT][exis]»iinLfoot_l_coord[4xis]; 
jpoin5LEG2][FOOTl[exii]« nm.feot_2_coord[exis]; 
jpoint[LEG3j[FOOT](exb] - nm.foot_3_coordlexis]; 
jpoint(LEG4][FOOT][exisi -= nniibot_4_ooord[exisl; 
jpoint(LEG5)[FOOT][exisi - nm.foot_S_coord(exis]; 
jpoiiitILEGd][FOOT](exis] = niafi) 0 t_ 6 _coordlexis]; 

/* footprint inhielize */ 
if(flag.phaseEnd = 1) { 

if (nni.leg_coiitect _fl e g [ U .Gl] *= 1) 

footprint{LEGl][axis] = nnLfoot_l_coord{ejdsl; 
if (nm.leg^cootect _fl eg[ l . E G21 =1) 

footpriiit[LEG2][exisl = nm.foot_2_coord[exisj; 
if (nm.leg_cootect_fUgfLEG3] = 1) 

footpriirt(LEG3][exisl = nm.foot_3_coord[exisl; 
if (nnLleg_contect_fleg[LKG4] = 1) 

footprint(LEG4][axis] = nm.foot_4_coord{exis]; 
if (nm.icg_cofitect_flegtLEG5] == 1) 

footptint[LEGS][axis] = nni.foot_5_coonl[axisj; 
if (nnLleg_contect_fleg[LEGd] = 1) 

footpfint(LEG6][axis] = nm.foot_6_ooord[exis]; 

} 

} 


/* Joint angle initialize */ 
for (leg“LEGl; leg<=LEG6; legt-*-) { 
for(joint=CB;joint<*FOOT;joint++) { 
jangIeneg]Doiot] = nm.inbdJoinl_anglenedtjoint]; 

oidiangleiieg][joint] =jangie[leg]Ooint]; _ 

//Tricky pert! inbdjoint angleG(HIP->KNEE2]{HIP,KNl,KN2,FT} 
//Tiidcy part! jangTentCB .>KNEE2HCB.HIP,KNl,KN2.Fr} 


} 

} 


dbm > vp.bodyspeed; 


/* to increment body position •/ 
bodypCW] += dbm*cos(wp.direction*DR); 
body[YW] += dbm*sin(wp.diiection*DR); 
bodylZW] += 0.0; 


/•to set next status*/ 
nni.body_center_coord[3] * bodypCW]; 
nm.body_center_cootd[4j = bodylYW]; 
nm.body_center_ooord[5] = body{ZW]; 

/• to calculate change in body position and euler angles •/ 
for (axis»=XW; axis<=ZW; axis-*-*-) { 

deuler[axis] = euler[axis] - oldeuler[axis]; 
dbody[axis] = body[axisi - oldbody[axisl; 

} 

/* to calculate diange in BODY position */ 
nm.bodymotion[0] = deuler[AZIMUTH]; 
nm.bodyinotion[l] = deuler{ELEVAT10N]; 
nm.bodymotion(2] = deuler[ROLL]; 
nni.bodymotion[3] = dbody[XW]; 
nm.bo(fymotion[4] - dbody[YW]; 
nin.bodymotion[5j = dbodyizWJ; 


/• to calculate joint angles •/ 

for (Ieg=LEGl; leg<=LEGd; leg*-+) { 







if(((wp.phue-=0)ft&((leg%2)’»0)) //^ ANDtxJd teg# 

K(wp.phue" I)A&((leg%2)>~ ]))){ //tpl AND even le^ 

/* compute FOOT poshion on «n ellipie curve respect to WORLD */ 
eUipsc(wp, footprint{Ieg], jpoint[leg][FOOT]); 

/* Foot Contact Caiiditioa*///TeiTaia Model =^rUt 
if (jpoiiltaeg][FOOT][ZWl > 0.0) { 

jpoint(leg][FOOT]pcW] = foo^print[leg]p(W] 
wp.stride*oos(wp.direction*DR); 
jpoirtfled[FOOTl[YW]» foo^jrinl(legl[YW] 

+ wp.stride*sin(wp.dtrection*DR); 

jpoint(leg][FOOT](ZW]>footpriiit[Ieg][ZW]; //O.O; 

} 

} 

/* Coordinates Tiansfonnatioo(BODY -tVORLD)*/ 
body_wortd(euler, body, jpoiid[Ieg][FOOT|); 

/* Inverse Kinematics */ 

inv_laneinatics(jpoint[]eg][FOOT], janglepeg]); 

> 

/* set joint angle */ 

fi)r(joint=CB;joiiit<”KNEE2;joiiit++) { 

for (leg^LEGl; leg<-LEGd; leg^-^) { 
nin.iuMJoint_anglePe^(joint] jangle[leg][joiiit]; 

//Tricky part! inbdJoint_angIeDIHn>->KNEEa] {HIP,KN1,KN2.FT> 
//Trickvpart! jangleQ[CB ->KNEE21{CB.HIP.KN1.KN2,FT} 

} 

} 

/* to calculate change in joint angles */ 
for (joint=HIP; joint<=KNEE2; joint++) { 

nm.legliiK>tioa[;oint>l] >= jangleII£Gl][)oint] • oldjanglelLEGlJIjoint]; 
nin.Ieg2niotion[ioint-l] jangle(LEG2][joint] • oidjangle[LEG2]{ioint]; 
nm.leg3niotioa[joiiit>l] =jangle[LEG3][joint] •oldjangle[LEG3]|joiiit]; 
nm.leg4inotioo[joint-l] = jangie[LEG4][joint] • oldjangle[LEG4]|joint]i 
nm.legSmotion{ioint>l] ■‘jangle(LEGSj[joint] •oIdjangle[LEG3][joint]; 
nin.Ieg$niatioa[ioint-l] ^ jangIe{LEGd][iomt] • oldjangIe[LEC6]Qoint]; 

} 

}/• End of cont_tripod_inotion •/ 

//*«***•«**•*«*«*••«*»••**.**«***••«•«*•«««•**•••««**•««*«••*««***•**•» 

// FUNCTION: ctp_foot0 (Center of the Tripod -> FOOT) 

void ctp_fi)ot(int tp_no. double ctpQ, double footOpCVZ], double tripodsize) 

{ 

double ratio = 3.0/2.0; 
intleg; 

for (leg“LEGl; leg<=LEG6; leg^-+) { 

if ( ((tp_no = 0) && (Geg%2) = 0))« 

((^_no = 1) && ((leg%2) =!))){ 
foot[leg]p<W] = ctppCW] + tripodsize*cos(ieg*60.0*DR); 
foot[leg][YW] = ctp[YW] + tripodsize*sin(leg*60.0*DR); 
footpeg](ZW] = ctp(ZW]; 

} 

} 

}// End of ctp_foot0 

// FUNCTION: foot_ctp0 (FOOT -> Center of the Tripod! 
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voidfoot_clp(ii<tp no, doubk c4>[]. doubk footQpCYZ]) 

{ 


ctppq “ (fbot[^_iK>]pC] + fbo»ltp_i»+2]pCl + fi)o»(tp_iio+4][X]>'3.0; 
ctp[Y] = (foo»(tp_no]pr] + fbol{tp_iio+2]m + fboH^_i»MJ[Y]y3.0; 
ct|i[Z] = (foot[tp_iio][Z] + fi>at(tp_D»4-2][Z] + foot[tp_iio+4][Z])0.0; 

}// End <rf' foot_ctpO 
// TITLE: pniit_tfatusO 

//FUNCTION: tcptintoutNait_iiicitioiidasvanablestatus 
//INPUTS: class Next Molkn, 

//OUTPUTS: None. 

// CALLED BY: MotionPUnoingO- 
//CALLS: None 

void print status(Next Motion Anm) 

{ 

intleg; 

printfOn—AQUAROBOT CURRENT STATUS—\n'); 
priiitflrPOSrnON\n"); 

printfl:” BODY Xw==?tf Yw-«tfZw==%I\n", nm.body_center_«>ofd{3]. 
nm.body_center_coord[4], 
ontbody oeater_coord[S]); 

prints FOOTl Xw=4tf Yw=9<i‘Zw=%fti", nnLfoot_l_eoofdpfW]. 
nmibot_l_coofd(YW], 
nmibot 1 coord[ZW]); 

prinlfr' FOOT2 Xw»?tf Yw-WZ»r=%fti", iimjfoot_2_coofdp{W], 
nmibot_2_ooord(YW], 
nmibot 2_ooord(2W]); 

prinffl’ FOOT3 Xw=%f Yw=WZw=?«\n", nmibot_3_coo»dp<W], 
iiinibot_3_oooed{YW], 
nuLfoot 3 oootd(23V]); 

printfC FOOT4Xw-^tfYw-WZw-%<\B^nn^Toot_4_coofdpfW], 
n«tLfoot_4_ooocd[YW], 
nmibot 4 oootd[ZW]); 

printir FOOTS X^v=%fYw-WZw-%^\n^nmibot_5_coo^dpCW], 
nnLfoot_5_cootdpn^, 
nmfoot 3 ooo(d[ZW]); 

printir FOOTS Xw=Stf Y\v=WZMr-%l\n", iimfoot_6_coordpcW], 
nm.foot_6_coord[YW], 
iimfoot_6_coordlZW]); 

printir JOINT ANGLE \n" ); 
fof^eg=LEGl; leg<=LEGS; leg++){ 
priitir LEG%dHIP=*/tfKl=%fK2=%Fn",leg+l. 

nm.inbdJoiiit_angle[leg][HIP], //HIP 
niainbdJoint_angle[leg](KNEEl], //KNEEl 
nm.inbdJoiiit_angle[leg]pOIEE2])'//KN£E2 

> 

prinUTFOOT CONTACT FLAG 'n’); 
for(leg*LEGl; leg<=LEG6; lefH-){ 

printir legHd^Sid", teg+l,nmleg_ooiitact_ilag(Ieg]X 

} 

printir'”"); 

}// End of piint_statusO 
// TITLE: priiit_walkpanO 

//FUNCTION: to print out WalkParameler class variables status 
//INPUTS: class Walkparameter. 

//OUTPUTS: None. 

// CALLED BY: MotionPlanningO- 









//CALLS: Nooe 

void print wmlkpara(WalkPaniiieler ftwp) 

{ 

prindOn—WALK PARAMETERS—\o''); 
prii)tfl[*PHASE~%(l\n*,wp.piiase); 

prinlirSTRlDE>%f DIR£CnON^A«a”.wp.stride. wp.difectioa); 
prinri(’TPSIZE«^FOTHEIGHT>*%^ii*,vvp.tripodsize, wp.footheiglit); 
printf(*BODY SPEEl>*f^',vvp.body3pecd); 

}// End of priid_walkpanO 
// END OF FILE "aitpgaitC" 

//FUNCTION: print_jDU daUQ 

void priiit_gnu_dria(Next_Motiaa Anm) 

{ 

dmible £-jler[3]; // Euler angle 

double body[3]; // Body Positiao 

double jangle[LEG][JOINTl> H angle 
double jpoint[LEG][JOINT]pCYZ]; // Joint Poeition 
int leg, joint; 

/* print for GNU plot data file */ 

FILE •IpO, *^>1. *^2; 

^ = fapcnCaitpl.datVa"); 

4>1 = fopenCe«y.dat',"w"); 
fi>2 ° fopeo(’eistable.dat’,‘w”); 

/* Initialization */ 

eulerfO] * nm.body_center_coord[0]; 
euletfl] * nm.body_center_ooord[l]; 
euler(2] = nfabody_center_ooord[2]; 
bodypOV] = mu.body__oaiter_coor^]; 
body(YAV]» nm.body_oenter_coord[4]; 
bodyfZW] = nfn.body_center_Goord[3]; 

/* joint angle initialize */ 
for Oeg-LEGl; leg<=LEG6; le^) { 
for(joinb=CB;joint<=FOOT;joint++) { 
jangle[Ieg][)oint] - nm.inbdJoinl_anglePeg][joint]; 

} 

} 

for (leg=LEGl; leg<-LEG6; leg++) { 

/* Kinemafics computation for GNU plot data file */ 
lcinematics(iangle[Ieg], jpoint[leg][HIP], jpoint[leg][KNEEl], 

jpoiii^eg][KNEE2], jpoiiitpeg][FOOT]); 

fof(joint=HIP; joint<=FOOT; joint++){ 


/* Coordinate Transforination(WORLD<-BODY) */ 
wofid_body(euler, body, jpoint[leg][joint]); 

Q>rintfi[ipO,''%f ^\n"Jpoint[leg][joint]pC]Jpoint[leg][joint][Z]); 

} 

ft)rijitftlpO,"\n"); 

} 

lprintl(lpl,"%f%f\n"jpoint[LEGl][FOOT]pCW]opoiiit[LEGl][FOOT][YW]); 

fi«ntf(fi»l,’«^f%f\n"Jpoiiit(LEGlJ{HIP]pCW]jpoirt[LEGl][HIPJIYW}); 

flprintl($l,'rrf%f\n"jpoiiit[LEG2]IHIP][XW]opoiiit[LEG2](HIP][YW]); 

%intf(^l,"\n"); 

fi?riiitfl:fpl,"%f%f\n"jpoiiittLEG2][FOOT][XW]jpoint[LEG2J[FOOT)[YW]); 

lpriiitfl;$l,"%f%f\n"jpointlLEG2][HIP]p{W]jpoint[LEG2](l3P]tYW]); 
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4irii<f(ft)l,'^Sf\B"jiK>ii<lIJG31[FC«TlpCWljpoirtILEG31[F^^ 

fl)riiifl:^l.'^%f\n"jpoirt[LEG3][HIP]pCW]jpomtILEG3][HIP](YW]>, 

ft)rii<<(4il.'^Sf\n"jiK)ii«(LECM)(HIP][XW]Jpoii<(U^ 

^)rirt^^l,'^Sf\n’jpoBtflLECMl[FOOTlpCWl.K»in‘a^lIFOO^ 

^)fii<«^)l,'^Sf\i»"Jpo«<P£G4)(HniP£Wljpo«<(LE04]^^ 

ft)rirtfl:tl.’^%f\n"jpoW{IJEGS]nWT|pCWj4pointIlJEG3J[FOOT][Y^ 

fi)rii<«ftil.'^?tf\n"jpoiiit(LEG5](HIP]pCW]jpoii<(IiG5]P^ 

^)rirtl(i5>l,'^3tf\n"jpoiBtpiG6][FOOT](XW]jpoiiip£C56inX)OT][YW]); 

4)«Bt^4»l,’^W\n"jpoiii«(IiG6](HIP]IXW]jpooi«piG6JP^ 

lpriiill(fpl,'^?tf'n"jpoiirttIXGl][HIP]pCWljp<)iiitI^ 

?4f \n", bodyfXW], body[YW]); 

ft>rintf(^2."Sf ?tf \n"4pon<n^Un'<>3TlPW]jpoiiit(IiGll[lWT][^ 
lpriim(^)2,"3tf \n”jpoiiit{IXG3]IFOOTIpCW]jpoiiit(LEG3I(FOOT][YW]); 

W\n"jpoiirt(IiG5](FOOT|pCW]jpoiiit(LEG51[FOOT)[YW]); 
fpnitS.fy2,’%[ 3tf^n"jpoilltiI^l]IFOOTlpOA^jpoilIt(IiGl][FOOT][YW]); 

^irirtf(^2,"3tf \n"j|)onitCLEG2][FOOT][XW)jp<>iiit(LEG2)(FOOTl[YW]); 
4)riiilfl:^2.-%f Sf\n"jpoint(l£G41[K)OT]p{W]jp<)ait(LEG4][FOOT][Y^ 
§>riiitf(i5)2,*%f \n"jiK)bit(lXG6][FOOTlpCW]jpoirtILEG«l[FOOTlpnV]); 
fpritff(lp2,"3tf W\n"jpoinlIl£G2](FOOTlpCW]jpointILEG2]{FOOT][YW]); 

ftirintfl:^2,"3tf \n", bodypCW], bodyfYWJ); 
fimilfl:fp2,"\ii*); 

fcl(»e(ipO); 

fclose(4>l^, 

fcloM(4>2): 

}/• End of priiit_Enu_d«UO 

n 

// FUNCTION: printJoint_daUO 

void print joint data(Next Morion &nni) 

{ 

/* print for GNU plot data file */ 

FILE •fpll, *^21, *^31, *^1. *4)51. ‘Ipei; 

FILE *^12, *^22, *^32, •^>42, •^52, *'r«2; 

FILE *fyI3, *lp23, *^33. •§>43, •§>5: . ^i^i\ 

§>I1 = fopen("legIhip.dat","a"); 

§)12 = fopenCleglknLdatV*"); 

§>13 fopenfleglkiG.datVa'); 

§>2I = fi)peoneg21np.dal’"a"); 

$22 = fopenneg2knl.datVa"); 

§>23 = fopeiineg2kn2.dal","»"); . 

§>31 =fopen(neg3hip.dat","a"); 

§)32 = fopeii(Teg3knl.dat”."a"); 

§>33 = fbpenneg3kn2.dalVa"); 

$41 = fopenneg41iip.dat","a"); 

§>42 = fopenCleg4knl.dal","a7, 

$43 - fopenneg4kn2.datV»"); 

§)51 =■ fopenOegShip-datVa"); 

§)52 = fopeiineg5tail.dat","a"); 

§>53 » fopenCleg5kn2.dal","a"); 

§)61« fi)penneg6hip.dat","a"); 

§>62 = fopeiirieg6knl.dalV«"); 

§>63 » fopeoneg6kn2.datVa"); 
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// print joiatuigle 

4itiiili(^ll.‘'%f\a*,nn.UidJoinl_aiigle(LEGl][HIP]); 

4>rintf(4>21.*Sf>ii', nnLiididJaint_aiig|e{L£G2](HIP]X 
4Kinlfl[4>31,*Sf l■n.illbdJoi^t_■ngleiL£G3j[HIP]); 

4irin(^^l,*^tf ^n^ nm.inbdJoiiit_iiigle[LEG4j[HIP]); 
4irinlfl[^Sl.'%f\n”.nni.inbdJomt_an^e{LEG3](HIP]); 

^rinti(4)61,*%f\o*, mtiiibdJoint_uigle[L£G6ilHIP]); 

\d”, nm.iiibdJoint_angle[L£Gl][KNEEl]>, 

4)rinUS[4>22.*%f \n', nm inhd_jomt_«ng|c[LEG2][KNEEl]); 

4)riiitfl[^2,*3tf \a", nmirintJoinI_angle(L£G3j(KNEEl j); 

4)iiiit^i^2,''3tf \n'', nm iiihd_joint_Mgle[LEG4][KNEEl]); 

4printf(l^32,''3tf \n''. nnLinbd_joiiit_uigU[LEGS][KNEEl]); 

4ptiiili(^2,'^\n'*, mn.inbdJoint_uigle[LEG6j[KNE£l]); 

%intf(^13,'^\n”. imi.iiibdJoiiit_«iigle(LEGl][KNE£2]); 

nnLinbdJoiiit_iiigle[LEG2](KNEE2]); 

4)nnlf|[4>33,”3tf \n’, nm.iiibdJoint_angle[LEG3j[KNEE2]); 
fpniitf(!^3,'^\a", iim.iiibdJomt_aiig|e[LEG4][KNEE2]); 

4)cinlfl[l^33,”%f \n", iim.iiibdJoint_in^[L£GS][KNEE2]}; 

4)tinifl[^3,'^\a*, mn.tnbdJoint_ing|e[LEG6][KNEE2]); 

fcIoie(4>ll}: fclosc(^12); fcla*e(4>13X 
fclote(fp21)-. fcl<we(fp22); fcl<»e(ip23); 
fclose(4>31); fcl<Me(1^32)-, fcloM(]^3); 
fclace(4>41^, fi:Ioce(fi>42); fctoce(4>43); 
fclose(4)31); fclase(fp32); fclose(fp33); 
fclosc(fp61); fda«e(4>62); fclose(fp63); 

)/* End of printJoiiit_dataO */ 

#ifedefARTPGArr_H 

#defineARTPGArr_H 

FILENAME; ar4)gutH 

^include "Kinematics.H" 
extern 

void get_goal(double Q); 
extern 

int get_menu(int); 
extern 

int get_footcfaoice(iiit); 
extern 

void gait_algorithm(Nex(_Motioii &, double Q, int, int); 
extern 

void robot_model(Next_Motioii &)', 
extern 

void disc_gait_planiiiiig(Next_Motion &, WxIkFarameler &, Flag &, double Q, int); 
extern 

void disc_set_flag(Next_Motion &, WalkPanmeter &, double Q, Flag &); 
extern 

void disc_tripod_motion(Next_Motion &, WallcParameter &, Flag &X 
extern 

void cont_gait_planning(Next_Motion &, WalkParameter &, Flag &, double Q); 
extern 
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void ooat_ict_fla((N«xt_Kloliaa ft, WalkPanunelcr ft, double Q, FUg ft); 
exteni 

void ocnt_1ripod_iiiatioa(N«a_Motioo ft, WeUcPannMter ft, FUg ft); 
extem 

void cl|>_fbot(tnl, double Q, double [][XYZ], douUeX 
extent 

void fbot_clp(int, double Q, double QPCYZ]X 
extent 

void iinnt_ititiu(Next_Mo(ioa ft); 
extent 

void priiit_waIlcpan(Wa)lcPennieter ft); 
extent 

void print_gnu_deta(Next_Moti«ti ftX 
extent 

void print Joint_data(Next_Motion ft); 

#endif 

//FILENAME: boLC 

//PURPOSE: This file makes X itiefc xpurobot psphics 
// interactive design 

// It utilizes Kinematic fimetions to determine xyz 

// ooonlimtrs 

// CONTAINS: fimctiacis shown in both 

// NOTE: This is aitd DUS 3D pro^am written in C-t-v 

// UPDATE: Last Update 1993 Apr 30 by Kenji Suzuki 

#include "gl-h* //graphics library 
#iitclude ’devioe.h” // graphics library file 

dittclude <stdioJi> //C-t-v library 

#iitclude 'botH" // declaration file 
trinclude'Link.H’ 

#inciude ’AbRigid-H" 
dinclude ’MatrixMy.H' 

#iitctude ’AbBody.H’ 

#iitclude ’KinematicaH'' 

#inciude 'AbLeg-H* 


/* added by Kenji Suzuki 1993 >^21 */ 

#iiK;lude 'arcansLir 
#iitclude "arfiittc.H'* 

#ittclude 'artpgaitH” 

maiitO 

{ 

//value relumed finm the event queue 
short value; 
kxtg maimnenu; 
long hhitem; 

FILE*ilp; 

Up - Ibpenrbotdat","!^; 

double goaipCY]; //goal position, WORLD coordinates 
Next_Molion actual, comrnattd, temp; 







int meou > 0; 
intfboisize 0; 

memi - get_inemi(incnu); 

get_go)a(*otl); 

get_footdioicc(foO»ize); 

// inhUlizc the HUS sydcm 
initUlizcO; 

// Create Pop Up Menus 
msinneiiu ■* nukethemeausO; 


//make tbe robot firom its pieces 
AquarobotBody aquabody. 

AquaLeg legl(aqiiabody,0.0); 

AquaLeg leg2(aquabody.60.0); 

AquaLeg leg3(a(piabody,120.0); 

AquaLeg lcg4(a<piabody, 180.0); 

AquaLeg leg5(aquabody440.0y, 

AquaLeg Ieg6(aipiabody300.0); 

Retuni_Coordiiiates coord; 

Passing_Iteiiis pass; 

Nexl_Motioa trails; 

intans; 

aiis“0; 

pass.le 9 Him ■* 9; 

coord > FiiidPositio 0 i(squabody, tegl, leg2, Ieg3, Ieg4, legS, leg6); 
trans = TfansferTo<3aii(coord, aquabody); 

while (TRUE) { 

// do we have something on the event cpieue? 


switch (qread(&value)) { 

case REDRAW: 
reshapeviewportO; 
break; 

case MENUBUTTON: // Menu selections 
if(value“ 1) { 

hititem > dopup(nuunmenu); 
prooessmenuhit(hititem); 

} 

bleak; 


caseAKEY: 

//the gait algoritfam is incorporated in this case 
// actii^ ° trans; 

//Something wrong! 

// "trans" does not give correct values. 
//byKenji 1993/4/30,15:00 


temp ~ actual; 


/• gait algorithm •/ 

gait_algorithm(temp, goal, menu, footsizeX 


command = tenq); 


/• robot model •/ 
robot_model(temp); 
actual = temp; 
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tmif ~ ooamiind; 


/•»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»•/ 


deJaz - traiii.bodyniotiaa{0]; 
ddd »lnn*.bodyina6aa(l]; 
ddroi • tniB.bodymotiao(2]; 
ddx - tnaf.badyniotiaa(3]; 
dely - tram.bodyiiiotioii(4]; 
delz •* tni]i.bod)aiotiao(S]; 

legl.MovelDCRmeaUl(«()uabody,tniis.ieglmoliao(0], 

ttam.legliiiotioa[l],tniis.Icglmotioa[2]>. 

le^.MoveIiicre iii c n U l( «qu«b o < < y.<rMg-l»g^»°°*^[01. 

tntB.leg2<iiotiao[l],trias.leg2iiiotioii(2]>. 

leg3.Movel i icmncnt«K«4M»<><^y.*f*°*-**t3incitioo[0). 

tniis.teg3inoti<n[l],tnm.kg3aiotifla(2]); 

Ieg4.MoveIncntneaUl(aquabody,tnuis.teg4ino(ioa[0], 

tniis.teg4aiotiao(l],truB.leg4iiioti<»[2]>, 

legS.MoveIiicniiieaul(aqiiabody,tnns.legSiiia(ioa{0], 

tnns.legSiiiacian(l].tmis.legSiiiolion[2]); 

leg6.MoveIiicRmenUl(aqiubo(iy,ttaiB.lcg6ino(ioii[0]. 

tnns.leg6nioliaa[l],1mis.leg6inotioo(2]); 

ooord - FiiidPo3itiafB(ax|uabody,Ugl.leg^,leg3,leg4,iegSJeg6); 

tnns = TraiisferToGait(coanl,aqiubody); 

break; 

caseRKEY: 

rewiDd(«fl>); 

break; 

} // and switcli on event queue item 
} //endif qtestO 


colo((BLUE); //background color 
clearO; 

// turn on Z-buffering 
2 buffer(TRUEX 

!' clear the z-buffitr 
zcleaiQ; 

buildnoaiiiovingviewingmatiui(vx,vy,vz,(fi( 4 fy^); 

drawaqua((coord.bodycX(coord.leglcX(coorltq^cX(coord.leg3cX 

(coord. leg4cX(cooi:d.i^cX(coard.Ieg6c)); 


/* tum z-buffenng off*/ 
zbuffer(FALSEX 

/* diange the buffers... */ 
swapbuffersO; 

qeiiter(AKEY,l); 


} 
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} //endc^maiii 


// FUNCTION: INTTIAUZEO 

// •••••••••••••••••••«•••*•«••••••••»••••••«•**••••• 

void initializeO 

{ 

// set up the peefened aspect ratio 
keepaspect(XMAXSCR£EN+l,YMAXSCR£EN-^l); 

// set up window size 

// ptt^)o»ilioo(700.0.1200.0.200.0.700.0); 

II preQwaitioaCZOO.0.1300.0,0.0,900.0); 
pre^>a«ition(200.0,7S0.0, SOO.0.900.0); // for video converter screen 

//open a window for the program 
wiDopen(*AquaRobot*); 

//make a title 
wiiUitle(*AquaRobot”); 

//put the nUS into double buffer mode 
dc^lebuffiaf); 

//configure the mis (means use the above command settings) 
gcottfigO; 

/* queue the devices*/ 

qd^ce(R£DRAW); 

qdevice(AK£Y); 

qdevicefPKEY); 

qdevice(RKEY); 

qdeviee(MENUBUTTON); 

vx« 0.0; 
vy = 400.0; 
vz= 800.0; 

r& = 0.0; 
rfy » 0.0; 
rfe * 0.0; 

} 


/•«•«*«•••*«*««*••••••••••• 

/* Function Make the_Menus 

/«»..«*••«•*****•.***.•*•*« 

long makethemenusO 

{ 

longtopmenu; 

long cameramenu; 

/• camera views •/ 
camerammi newpupO; 
addtopup(cameramenu,"Camera View %t"); 
addtopup(cameramenu,"ABOV£ %x21 LEG5-d VIEW %*3"); 
addtopup(camerainenu,"LEGl VIEW Hx41LEG4 VIEW »/«t5"); 
addtopup(canieramemi, ''LEG2>3 VIEW %x9"); 

// build the top level menu 

topmenu- defi>up(”Roll Off Side 34t| Camera %xl 9tei|FileRead %x6 |ResetFile %x8|KeybdRead %x7|Reset %xl4| Exit 
%x 13 ".cameramenu); 

//return the name of this menu 
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reiiiniCtaimeau); 


} 


/••»•••*••••••••••••••••••*•••••••••••••••••••••••••••••••••••••••••••*/ 

/* Function Prooen Menu Hit */ 

/m***9^0000**0^*mm*9**9**—9»m*m*mm*m99**9m*—9*—*^*9—09***0*0***/ 

void pracennienuhitOoiig hhitrai) 

{ 

twitcli Oiititem){ 

caie CAMERA:/* top slide ^menu */ 
hteak; 

case ABOVE: //Graphics Cootdinates 
vx “ 0.0 + ifit; 
vy -1000.0 + 1 ^ 
vz “ 0.0 + rfe; 
bleak; 

caseLEGS6_VIEW: /• View from the behind robot */ 
vx “ 0.0 + rfic 
vy - 400.0 + rfy, 
vz “ -800.0 + rfe; 

bteak; 

case LEG1_VIEW; 

vx =* 800.0+ ffic; 
vy - 400.0+ rfy; 
vz ” 0.0 + tfc, 
break; 

caseLEG4_VIEW: 

vx «-800.0+ ffic; 
vy = 400.0 + tfy, 
vz “ 0.0 + ife; 
break; 

caseFILEREAD: 

qenteKPKEY.l); 

break; 

case RESETFILE: 

<)eiitet(RKEY,l); 

break; 

caseLEG23_VIEW: 

vx = 0.0 + ific; 
vy - 400.0 + tff, 
vz = 800.0 + rfe; 

case RESET; 

vx - 0.0; 
vy = 400.0; 
vz = 800.0; 
break; 

case EXIT; 

exit(0); 

break; 

} // End Switch 


} 

/* for otqects that are in the same coordinate system but arent moving 
with the contiiMious rotationsAranslatioiis/scaliiigs, we use this 
routine... 

•/ 
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// FUNCTION; BUILONONMOVINGVIEWINGMATRIX 


*««•••«••*•«•«««««««««««««*•«••«««•• 


void buildDoanoviiigviewiiigaiatnx(float vx^loit vy^loal vz, 
refic^loat refy^loat nb) 


// we mutf call loadunit befote we get the prcjection 
// and viewing stuff... 

loadunitO; 

//just call the per^iectiveviewing matrices 
projectiooandviewingmatrix(vx,vy,vz,refic,fefy,feff); 


/* put up the projection and viewing matrix */ 

// FUNCTION: PROJECTIONANDVIEWINGMATRIX 

void projectionandviewingmatrix(float vx^oat vy^oat vz, 
float refic,float refy^loat r^) 


// peispective projection 30 for the world coord sys 
// the near and hr values are distances from the viewer 
II to the near and ftf clipping planes. 

// We ate at (vx,vy,vz) and looking towards 
// the center point of the object. 

// (towards (re&,refy,re&;)). 


perspcctive(450,1.25,NEARCUPPINGJFARCLIPPING); 

looi^vx,vy,vz,re£(,refy,re£c,0); 


//this routine loads a unit matrix onto the top of the stack 

void loadunitO 

{ 

static float un[4][4] = { 10,0.0,0.0,0.0, 


//load the matrix 
loadmatrixfun); 


0 . 0 , 1 . 0 , 0 . 0 , 0 . 0 , 
0 . 0 , 0 . 0 , 1 . 0 , 0 . 0 , 
0 . 0,0 0 , 0 . 0 , 1.0 ); 


// AQUA DRAWING 
void drawaqua(double *bodyc. 


colotfWHITE); 

linewidth(3); 


double *leglc, double *leg2c, double *leg3c, 
double *leg4c, double *legSc, double *leg6c) 


//NOTE!!!!!!!!! 

// +x to right, +y up, +z out of screen ->for graphics 
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// +x out of legl, +y out of screen, -z dowi»->for equerobot 

// X z y 

move(bodyc{3],-bodyc(3].bo(lyc[4]X 

draw(bodyc{6],-bodi^8].bodyc[7]); 

dnw(bodyc(9].-bo<^l l].bo<lyc[IO]X 

draw(bodyc{12],4>odyc(14],bodyc[13]); 

drew(bodyc(15),-bo<^171,bo<^16)X 

dtmw(bodyc(18].-bodyc{20].bodyc{19]X 

tewi(badyc[3],4>odyc[S].bodyc{4]X 

//drawee line fixxn body center to iegl joint \ 

linewidlfaflX 

niove(bodyc[0],-bodyc[2],bodyc(l]X 

draw(bodycP),-bodyc(SJ,bodyc(4]); 

//draws legl 

oolofOfELLOWX 

linewidthfSX 

// X 2 y 

move(leglc[0],-lcglc[2].leglc(11); 

draw(leglc{3].-Ieglc[51Jeglc(4JX 

draw0eglc(6],-leglci8],lcglc[7]); 

draw(leglc[9j,-teglc(l l]Jeglc[lO]); 

//draws ieg^ 
ooia(<GREEN); 

Iinewidlh(SX 

inov«(le^c[0],-leg2c[2],Ieg2c( 1 ]); 
draw<Ieg2c(3].-leg2c[S].le^c(4]X 
drawaeg2c{6].-le^[8].lcg2c[7]X 
draw(Ieg2c{9].-leg2c[l ll,leg2c[10]); 

// draws leg3 
colorfGREEN); 

IiDewi(ldi(5); 

inove(leg3c(0].-leg3c[2]Jeg3c(l]); 
draw<Ieg3cP],-leg3c[S]Jcg3c[41X 
drsw(leg3c(6],-leg3c[8],leg3c[7]X 
draw(leg3G[9].-leg3c(l l]Jeg3c[10]); 

// draws leg4 

coloifGREEN); 

linewidUi(3X 

move(]eg4c(0],-leg4c(2]Jeg4c{l]X 
(baw(Ieg4c{3],-leg4c[S],leg4c[4]}', 
draw^eg4c{6],-leg4ci8],leg4c[7]X 
draw^eg4c(9i,-leg4c[l l]Jeg4c[10]); 

// draws legS 

color(GR££N); 

iinewidlh(SX 

inove(Ie^c[0].-leg3c[2],leg3c[l]); 
draw(leg5c(3],-leg5c[5],leg5c(4]); 
draw^egSci6j,-legSc[8]Jeg3c[7]); 
draw0egSc[9j,-leg3c[l l],IegSc[10]X 

// draws leg6 

coloifGREEN); 

liiiewidth(3); 

inove(legfo[0],-leg6c[2],leg6c[l]); 
draw(leg6cp],-leg6c[S],leg6c(4]); 
draw0eg6ci6].>leg6c[8j,leg6c{7]X 
draw<Ieg6c[9],>leg6c|l I],leg6c[l0]); 


} 
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11 »<»««»»»»»> 

// FILENAME; boLH 

// PURPOSE: de&M* OMMtann and fiiuctioBi used iot botC 
// NOTE: TIus is an IRIS 3D program written in C-*-*- 


#ifcdef BOT_H 
#define_BOT_H 

// provides conattnls for menu pr o c e ss ing opcions 

Mefine CAMERA 1 

#defiiie ABOVE 2 

#defiDeL£G56 VIEW 3 

ddeEneLEGl ^W4 

#define LEG4_VIEW S 

#define FILEREAD 6 

#de&w RESETFILE 8 

#de6ne LEG23_VIEW 9 

#defineR£S£T14 

Mefine EXIT IS 

Mefine NEARCUPPING 10.0 // planes defined 
// #define FARCUPPING 1023.0 

#defiiie FARCUPPING 2047.0 // changed 14MAY93; C.Schue 


long maketbemenusO; 

static float itcj* reference point on in the x direction */ 
static float tfyj* reference point on in the y direction */ 
static float ticj* reference point on in the z directiao */ 

static float vx; /* view point on in the x direction */ 
static float vy, /* view point on in the y direction */ 
static float vz; /* view point on in the z direction */ 

double deltal,delta2,delta3; 
double delaz,delel,delrol.delx,dely,delz; 

void proccssroenuhitflong hititem); 

void initializeO; H initializes graphics layout 

void loadunitO; H a unit matrix used in rotatioWlranslation 

void pfojectionaiidviewingniattix(float vx. float vy, float vz, 
float refit, float refy, float refit); 

void buildnomnovingviewingniatrixffloat vx, float vy, float vz, 
float refit, float refy, float refo); 

void drawaquafdouble *, double *, double *, double *, 

double *, double *, double *); // includes all legs and body 


#eodtf 

'/ niENAME: Kinematics.C 

//PURPOSE; todetenninepositicos(x,y,z)fiomtheH_matrix 
// to read from a file the new link angle dianges 

// and update the appropiate leg/link values 

#include <inath.h> 

#include <stdlib.h> 

#include <stdioJi> 

^include "MatrixMy.H" 
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#include ’AbLeg.H* 
^include 'AbBody.H" 
tfiiichide ’LudoH’ 
#tiidii(ie ’LinkO.H' 
iWnchide *KiiienMlici.H* 


^include’arcaosLH" 


iMefine GROUNDELEVATION 0.0 


//xyz coordinates are determined fixm the H_iiiatrix and 
//return the coordinates using the Relum_Coordinates structure 

Retum_Coorditudes FindPositioiis(A<|uarobotBody &hody, 

&Iegl, AquaLeg &le^, AquaLeg &le^, 

&leg4, AquaLeg &legS, AquaLeg &leg6) 

{ 

Retum_Coordinates *rc; 
rc = new Retum_Coordinates; 

//body center coordinates 
rc->bodyc(0] =* body.body_list->val(0,0); 
ro->bodyc[li = body.body_list->val(0,l); 
rc->bodyc[2] = body.body_list->val(0,2); 

//body points to draw 
rc->bodyc[3] * body.body_list->val(l,0); 
rc->bodyc[4]» body.body_list->val(l,l); 
rc->bodyc[5] *body.body_Ust->val(l^); 
rc->bodyc[6] •= body.body_list->val(2,0); 
rc->bodyc[7] = body.body_list->val(2,l); 
rc->bodyc[8i = body.body_list->vaI(2^); 
rc->bodyc[9] = body.body_list->val(3,0); 
tc->bodyc[ 10 ] “ bo^.bo^_list->vaI(3,l); 
rc->bodyc(lli « body.body_iist->val(34); 
rc->bodyc[12] * body.body_list->val(4,0); 
rc->bodyc[13] = body.body_list->val(4,l); 
rc->bodyc[14] = body.body_list->val(4^); 
re->bodyc[15i =body.body_list->val(5,0); 
rc->bodyc[16] = body.body_list->val(5,l); 
rc->bodyc[17] = body.body_list->val(5,2); 
rc->bodyc[18] = body.body_list->val(6,0); 
rc->bodyc[19] = body.body_list->val(6,l); 
rc->bodyc[20j = body.body_list->vaI(6^); 

// prints out body coordinates 

/• 

printf(Tx)dy center %3f, %3f, %3f \n"jc->bodyc[0]4c->bodyc[l], 
rc->b^cp]); 

printflTbody pt 1 %3t 343L %3f \n"^>bodyc[3]^>bodyc{4], 
rc->b^c[5)); 

prind^''body pt 2 %3€ %3L A3f\n"jc->bodyc[6]^>bodyci7], 
rc->bodyc[8]X 

printfC"^ pt 3 %3t. %3£ %3f \n"jc->bodyc[91^>bodyc(10], 
rc->bodyc[ll]); 

printirbody pt 4 %3L %3£ %3f \n"jc->bodyc[12]jx>>bodyc(l3], 
rc->bodyc[14]); 

printf|>>dy pt 5 343^ %3L %3f \n"^>bodyc[15]^>bodyc[16], 
ro->b^c(17J); 

piintf("bo^ pt 6 %3L %3C %3f\n>n"^>bodyc[l8]jc->bodyc[19], 
rc->b^c[20]); 

•/ 


AquaLeg 

Aqualjeg 


//joint one teg coordinates: [0]=x [l]°°y [2]=n. 
rc->leglc[0] = legl.linkO->H_niatrix->vJ(0,3); 
rc->IegJJc[0] = leg^.link0->H_matrix->val(0,3); 
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rc->lcg3c[0]»le^.link0->H_miliix->vil(03); 
ro->lq4c[0]»leg4.1inlcO->H_in«lrix->wl(03)'> 
re-»egSc[0] - teg5.link0->H_inatrix<>vtl(03); 
n»l«g6c[0] * leg6.1iiikO->H_initrix->vi](0^); 
i«->leglc[l] - legl.UnkO->H_iiMtrix->v»l(l^); 
i«->ieg;2c[l j - leg2.liiikO->H_i>Mtrix->vi](14); 
rc->leg3c[l] > teg3.liidcO->H iiutfrix->vi^l3); 
rc->leg4c{l] - leg4.1iiikO->Hlinatrix.>val(1.3); 
io->legSc[l] • Ieg5.1inkO->H_iDattix->vil(13); 
i«->leg6c[l]»leg6.1iiik.0->H_in*trix->val(13); 

ic->leglc[2] - legl.linkO->H_iMlrix->v«l(23); 
fc->leg2c[2] = leg2.1inlcO->H_iMJrix->vil(2,3); 
rc->leg3c{2] = le^.liiik(J->H_mitrix->v*l(23); 
rc->leg4c(2] = leg4.liiikO->H_inalrix->viI^^); 
rc->leg3ci2] * leg3.1inlcO->H_iB»lrix->vil(23); 
fc->ieg6c[2] = leg6.1iiikO->H_marix->v»l(2^X. 

// inboard joint angle info. HIP Joint angle 

// DD * Pedlioi”*] 

rc->inbd_ioint_angle[0][l] = legl.tinkO-><jetInboardJotatAiigleO 
rG->inbdJoint_angle[l][l] “ leg2.linkO-><]etIiiboanUoiiitAng|eO 
rc->inbdJoint_angle[2][l] leg3.1ink.0->GetlnboanUoiotAngle0 
rc->inbdJoiin_ang|e(3][l] = leg4.1inkO->GetInboardJointAngleO 
rc->inbdJoint_ang|e[4](l] = legS.linkO->GetInboardJointAngleO 
rc->inbdJoint_angle[3][l] = leg6.linkO->GetInboanUointAngleO 

//joint 2 x,y^ coordinates (3]=x (4]=y (5J«z 
ro->leglc[3] = legI.linlcl->H_matrix->^04); 
rc->teg2cP] = ieg2.linkl->H_ntttrix->val(03); 
rc->leg3cP] = ieg3.1inkI->H_matiix->val(0^); 
ro->ieg4cP] = leg4.linkl->H_o>atrix->val(0^); 
io->leg3cp] = legS.linkl>>H malrix->val(0^); 
fc->leg6cp] = leg6.linIcl->Hlinatiix->val(0^); 
re->leglc{4] ■= legl.linlcl->H_malrix->vaI(l^); 
rc->leg2c(4]»leg2.iinkl*>Hjnat)rix->val(14); 
rc->leg3c[4] * leg3.litdcl*>H_inatrix>>val(14)'. 
rc->ieg4c[4)»leg4.liiilcl^H_matrix->val(l^); 
ro->legSc(4] = tegS.linkl->H_inatrix->val(l^); 
rc'>teg6c[4] * leg6.linkl->H_inatrix->val(13); 

rc->leglc[S] = legl.linkl->H_n>atrix->vil(23); 
iiDO>i^c(5j = leg2.linkl->H_matrix->val(2^); 
rc->leg3c(5] = leg3.linlcl->H_matrix->val(23); 
tc->leg4cP] * Ieg4.linkl->H_maIrix->vaJ(2^); 
rc->leg5c(5] = leg5.iiidcl->H_nutrix->val(2^); 
rc->leg6c[S] = leg6.linlcl->H_o»atrix->val(2^); 

// joint 2 malioa_liinit_flag 

rc->niotion_limit_ilag[0] - iegl.linicl->GetMotionLiniitFlagO'. 
rc->motion_iinut_fUgP] = tegZ.Iinkl->GetMotionUmitFlagO; 
rc->motioa_]iniit_flag[6] “ leg3.linkI->GetMotionIjniitFlagO: 
tc->motionJiniit_flag[9] = leg4.linlcl-X3etMotiooLiinitFlag0‘. 
ic->inotion_liniit_flag(12] = iegS.linlcl->GetMotionLiniitF1agO; 
rc->n)otion_limit_ilag[13] = leg6.1inkl*>GetMotionLiniitFlagO; 


// inboard joint angle info. KNEEl joint angle 
// DD = PegjOoint] 

rc->inbdjoint_angle[0][2] - legl.linkl->GetInboardJointAngleO 
rc->inbdJoint_angle[l]p] = leg2.1inlcl-><jetInboardJointAngleO 
rc->inbdJoint_angle[2][2] = leg3.linkl->GetInboardIointAngleO 
rc->inbdJoiiit_anglep][2] = leg4.linkl->GetInboardJointAngleO 
rc*>inbdJoint_angle[4][2] = legS.linkl-XjetlnboardJointAngleO 
n>>inbdJoiiit_angle[5][2] = leg6.linkl->GetInboardIointAngleO 

//joint 3 xyz coordinates [6]*x (7]=y [8]=z 
rc->leglc[6] = legl.link2->H_niatrix->^(03); 
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rc-^et^6] • leg3-l“>k2'^H_ni*trix->vil(0^); 
rc-»eg3c{6] - Ieg3.1ink2->H iiiatrix->val(0^); 
ic->leg<c(6] • leg4.Uiik2o^H_BMlrix->val(03); 
ic->l45c(6j • ieg5.1^->H_iiutnx->vaI(0^^> 
ro^leg6c[6] - leg6.Iiiik2->H_milrix->v«l(0^); 

rc->kglcr7] - Iegl.liiik2->H iMlrix->v«l(UX 
re->teg2c{71 - leg2.1ink2->H_iii»lrix->v«l(U>. 
rc->leg3c[7] =• l«g3.1iiik2->H_iiBtfrix->val(U); 

to->le^[7] = leg4.1iiik2->H_ina«rix->vil(14); 
n>>lcg3c[7]»tegS.liiik2->H in«trix->v«l(l^); 
ro->leg6c(7] = leg6.!iiik2->Hlinalrix->val(l^); 

ro->leglc[8] = legl.yiik2->H_in*ltix->val(2^); 
ro->leg2c[81 >= leg2.1ink2->H_mittix->vil(2^); 
ro->leg3c[8] “ leg3.1ink2->H_m*lrix->val(24); 
rc->Ieg4c[8] = leg4.1iiik2*>H_iiiatrix->vaI(2^); 
rc->leg3c[8] = leg5.1ink2->H_iii«lrix->v»I(24); 
ic->leg6c[8] = leg6.1iiik2->H_iiiilrix->v»l(24); 

// joint 3 motioa_liniit_flsg 

rc->iiiotion_limit_flag[l] “ legl.Unk2->G«tMotiooLiinitFUgO; 
rc->inolion_liniit_flng[4] * leg^.link2*^GctMotiooLiniitFlngO; 
io->iiiati<Mi_limftjQag[7] = legS-link^-^GetMotiooLmiitFlngOi 
rc->iiiotioa_liinit_fUg(10] * leg4.1ink2->GetMotionLi(nitFUgO; 
rc->matioa_limit_flag(13] ” leg5.1ink2->0€tMotiooLiinitFI»g(); 
rc->mo(ion limit_fUg(16] leg6.link2->GetMotionLimitFUg(); 

// inbonrd joint angle info. KNEE2 joint angle 

// nO”P«8l[i“^] 

rc->iidxijoint_ang]e[0][3] ” legl.link2*>G*tInboaniJointAngleO 
rc->inbd_Joint an^jljP] ^ leg2.1ink2->GctInboanlJointAngieO 
rc->inM_joint angte[2][3] Ieg3.1ink2->GctInboardJoinLAngleO 
rc->iftbdJoint~ang|e[3][3]»leg4.1ink2->GetInboardJointAngleO 
rc->inbd_joint_ang|e[4][3] * leg5.liiic2->GetInboaidIointAngleO 
ro->iiibdJaint~aqglef5jpj “ kg6.1iiik2o><3dInboardJointAligleO 

//joint 4 xyz coordinates [9]'=x [10]*y [11]“=2 
rc->teglc(9]»legl.l'tik3->H_niatrix->vaK0^); 
rc->le^cf9] = le^.link3->H_inalrix->val(0^); 
rc->teg3c[9] * leg3.1ink3->H_niattix->VBl(0^); 
fc->leg4c[9] “ leg4.1ink3->H_inatiix->val(0^); 
rc->leg3c[9] = leg3.1ink3->H_matrix->val(03); 
rc->leg6c[9j = leg6.1ink3->H_niatrix->val(0^); 

rc->leglc[10] • legl.link3->H_niatrix->val(l,3); 
rc->lej{2c[10] = le^.link3*>H_niatrix->vil(l^); 
rc->leg3c(10] = leg3.1ink3->H_niatrix->val(U); 
rc->leg4c[10] * leg4.1ink3->H_matiix->vaI(l^); 
ro->legSc[10] = leg5.1ink3'>H_matiix->val(l^); 
rc->Ieg6c[10] = leg6.liids3->H_niatiix->val(l^); 

rc->leglc[ll] = legl.link3.>H_inatrix->vaI(2^); 
rc->leg2c[ll] = Ieg2.link3->H_matiix->vaI(2^); 
rc->leg3c[ll] = leg3.Iink3->H_niatrix->val(2^); 
rc->leg4c[l 1] = leg4,link3->H_niatrix->vil(24); 
rc->legSc[ll] «leg5.1ink3->H_niatrix->vil(2^); 
rc->Ieg6c[ll] = leg!6.1ink3'>H_matrix->vil(2^); 

//joint 4 motion_Iimit_flag 

rc->niotion_limit_flag[2] “ legl.link3->CjetMotionLiniitFlagO; 
rc*>nxitioa_Iinut_fiag[S] = le{^.link3'^OetMolionLiniitFlagOi 
rc->niotion_liniit_fIag[8] = leg3.1ink3->C5«tMotionLunitFlagO; 
rc->rootion_limit_flag[ll] = leg4.1ink3->C5etMotionLiniitFlagO; 
rc->motioo_lin)itjQag(14] = leg5.1ink3*>GetMotionLimitFlagO; 
rc->niotion_lin>it_llag[17] = leg6.Unk3->GetMotionLimitFIagO; 
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// inboaid joiot logic info. FOOT joiot angle 

II DD " 

rc->inM_)oiHt_angle[0][4] • legl.link3->GctInboardJointAn^eO; 
ic->inbdJoiat_angte[l][4] • leg?.link3->GctInb(MidJointAngleO; 
rc->inbdJoia(_angle[2][4] ~ leg3.link3->GetInboardlointAo^eO; 
ro->inbdjoint_iog|e[3][4] - lcg4.linlc3->GetInboanUointAngle(^, 
ic->tnbdJoint_angle[4][4] - leg5.linlc3->GdInhoardJointAn^eO', 
ro->tnbdjoiiit_angle[3][4] - legiS.link3*>GetInboardJointAngleO; 

// teat fof mpporting legi and idjialing lcg;_Mpport_flig 
if (fobi(ic->teglc[ll]) GROUNDELEVATION) legl.SelLegSu]ipoftFUg(l); 

else legl-Setl^egSupportFIagfO); 

if (&l)e(n!->leg2c[n]) >> GROUNDELEVATION) leg2.SetLegSuppoctFlag(l); 
elM le^.SetLegSupporlFlag(0); 

if (fia)a(re->leg3c[U]) >“ GROUNDELEVATION) leg3.SetUgSuppo»tFlag(l); 
else leg3.Se(LegSupportFUg(0); 

if (fabi(fO->leg4c[ll]) >- GROUNDELEVATION) leg4.SetLegSuppoftFlag(l); 
else le^.SetLegSuppoftFlag(0^ 

if (fobs(re->legSc[l 1]) >~ GROUNDELEVATION) leg3.SetLegSuppoftFlag(l); 
else leg3.SetLegStq)pOftFlag(0); 

if(&bs(n»leg6c[ll]) GROUNDELEVATION) leg6.SetLegSuppoftFlag(l); 
else le^.SetLegSuppoftFIagfOX 

// places leg;_suppo(t_fUg into rc 
rc->ieg_support_flag(0] ° legl.GetLegSuppartFlagO: 
rc->leg;_supj>oft_flag[l] - leg2.GetLegSiq)poftFlagO; 
ro->leg;_suppart_flag[2] = legJ.GetLegSupportFlagO; 
rc->leg;_sitppott_ilag(3] = leg4.GetLegSiipportFlagO; 
rc->leg;_suppaft_ilag[4] • legS.GetLegSupportFIagO; 
rc>>leg;_suppoft_flag[3] <• legS.GetLegSuppoftFlagO; 

// prints body and leg xyz ooordioates 

/• 

int tow, ool; 

ptintfl[*lcgl leg2\o'‘X 

fof( row = 1; row<3; row++){ 

for(col • 3; ool>0; col-) 

printfl;’^.4f "4C->leglcP * row • colj); 
pni^C 7. 

fof^col = 3; col>0; cd-) 
piiiitf(*%6.4f "4»^leg2c[3 • row • col]); 
priiitf|7\n7, 

} 

ptwtflr'“7 

printflT^ Ieg4>n"): 

foifrow ” 1; row<5; row++){ 

forfcol “ 3;col>0; col-) 

printH’'346.4f "^>leg3c[3 • row - col]); 
printfC y, 

foi< col “ 3; ool>0; col-) 

priiitfl;"%6.4f "^>leg4cp • row - col]); 
printf("\n”); 

} 

printfl7\n'>, 

printfl["leg5 leg|6'n'); 

forfrow = 1; row<5; row++){ 

fof^col “ 3; col>0; col-) 

printlC^ftS.df "^>leg5cp * row - col]); 

printr "); 

for(col = 3; ool>0; col-) 

printfC^d.df "4ic->leg6cP * row - cot]); 
printf("\n"); 

} 

printf|["\n7, 

•/ 

return •rc; 

} 
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//FUNCTION: rile_U$e 
//PURPOSE: rcttb dewMi leg dMnges from a file 
//INPUT: reedi fiom file: Ie|^, deitel, deiU2, <lelU3 
// OUTPUT: celctiletw new le^Unk ooardinatet (mienial) 

Paating;_Iteim File_Use<FILE *i4>> A<|iiarobotBoily Abody, 

AquaLeg ftlegl, AiiuaLeg &le^ Aqual^ &leg3. 
AquaLeg &lcg4. AquaLeg AlegS, AquaLeg &leg6) 

{ 

Passing_Itenis *pass; 
pass new Passiag_Items; 


ficanf(i%'9id %lf %lf t«lf Slf Sif Sif .ftpass->body,&pass->dell. 

ftpass->del2.ftpasa-^l3,&pa»->deI4,&pa»->delS,&pass->del6); 


if (pass>>legnuni < 9) { 

body.MovieInarmfnlal(pats->dell. pass->det2, pass->det3, 

pass->del4. pass->delS, pass->del6); 


ficaiif(i4>.*%d SIf Slf 9Uf. 

&pass->legniim, &pass->dell, ftpass->del2, Apaas->del3); 
legl.MovelncrenieoUl(bodyjiass^>dell4>ass-^l2,pass->del3); 
fican^iip.'^id %tf SIf Stf. 

&pass->leg|iuin, ftpass->dell, &pass->del2, &pass->del3); 
leg2.MoveIncran«Ul(body,pass^>dell4iaes-^l24>ass-:^l3); 
ficaiifi;i4>,**/4d ^Slf 9Ur, 

&paas->legnuin, &pass->dell, &pass*>del2, &pass->deD); 
leg3.MoveInGranental(bodyj>ass->dell4>ass-:^12,pas5->deU); 
ficaiifi:ifp,'^ %if WSIf, 

&pass->le9ium, &paas->dell, &pass->del2, &pass->deU); 
leg4.MoveIncranea<al(body4)ass->del 1 jMss-::^124>sss->deD); 
fiKaiif(i4>.'%d «4lf WSir, 

dipass->legniiin, &pass->dell, ftpass->del2, &pass->del3); 
leg5.MoveIncRnieotal(body,pass->del 1 j)ass-:^l2,pass->del3); 
BauU(Up,-%d HIT, 

d^>ass->legniini, ftpass->dell, &pass-><ieI2, &passOdeD); 
le^.MoveIncraneidal(body4>ass->del 1 jwss-^l24>ass->del3); 
ficanfTiQi.'fid %lf %lf SIT, 

ftpass-Megnum. ftpas»'>dell, &pass->deI2, &pass->deD); 


if (pass->legiiiim = 0) { 
pass->dell = 0.0; 
pass->del2 - 0.0; 
pass->del3 = 0.0; 

}; 

}; 

retuni *pass; 

} 


//FILENAME: Transfia-ToGait 

//PURPOSE: places the body center and leg coordinates in a 
// Next Motion stnicture fix gait algorithm use 

Next Motion TransfirToGait(Retura Coordinates &ooord, AquarobotBody Abody) 

{ 

Next_Motion •terap; 
tenq> = new Next_Motion; 


tenq)->body_center_coord(3] = ooord.bodyc[0]; 
temp->body_oenter_ooord[4] = ooord.bodyc[l]; 
tenq>->body_oenter_coord(5j = coord.bodyc[2]; 


tenq>->lbot_l_coord[0] > coord.leglc[9]; 
temp->foot_l_coord[l] = coord.Ieglc[10]; 
temp->fi>ot_l_coord[2]« coafd.leglc[ll]; 


224 








tciiip->feol_3_cooni(0] • coonllej;2^9]; 
tanp-^>foat_2_caoni{l] - coard.lcg2c{10]; 
tcai|>->fi>at_2_ooard[2] •• ooard.le^c(ll]; 

teDip->feot_3_oocrd{0]» ooanlleg3c{9]; 
tcmp->fi>at_3_ooard(l] - ooatdlcg3c(10]; 
tcin|>->foot_3_ooafd[2] > ooard.le83c(U]; 

taiip->fi)ol_4_ooad{0] > ooatdleg4c{9]; 
tanp-^>foot_4_coanl(lj • oaonLleg4c(10]; 
tcnp->fi>at_4_coard[2j - oaard.leg4c[ll]; 

teiiip->foot_S_coaiid[0] • ooord.legSc[9]; 
tetnp->fi>ot_5_coo(d(l j • coordlegSc(10]; 
tcmp->foat_3_coanl(2] = coord. teg3c(U]; 

tein|>->feoC_d_coofd{0] - cootd.ieg6c(9]', 
tcnij>'>feot_6_coani(l] > coard.lcg6c{10]; 
tcnif>->foot_6_coord{2] - coonLlcgdcIll]; 

// get joint angle values from algoritfam module 
for (int i-0; i<d; i++) { 

for (ini j"0; j<4; j++) // HIP -> FOOT 

temp^>inbdJoint_angle[i][j] = coord-inbdjoiid attgle[i][i]; 

}; 


// current body elevation 

ten)p->body_oenter_coord(l]«-1. • body.H_matiw->val(2,0); 


// current body azimuth 

teinp->body_oeater_ooord[0] >asin(body.H_matrix->val(l,0)/ 

~ cos(temp->body_center_coord[l])); 

// current body roll 

teii^>body_cencer_coord(2]=asin(body.H_malrix->val(2,l) / 

cos(t*nip->body_ceiiter_eootd(l ])); 


// joitd_limit_flag 

teinp-^oint_timit_flag(0] • ooaidjnotioa_liniit_flag(0]; 
temp-^int_limit_fiag[li <■ cootdjnolica_limit_ilag(li; 
tenip-^oint_liniit_flag[2] * oootd.molion_limit_flag(2]; 
tenip-^oiid_limit_flag[3] oooid.mattoa_limit_flag(3 j; 
temp->joint_limit_fIag(4j - ooard.matiaa_limit_fiag(4j-, 
tan(>->ioint_litnit_flag(Sj ~ ooord.iiiotioa_liniit_flag(3j; 
temp-^oint_liniit_fUg[6] - ooord.malion_limit_flag[6]; 
temp->joint_limit_flag(7] - ooord.motian_limit_ilag(7]; 
temp->joint_limit_flag(8] - coord.mocion_timit_ilag[8]; 
temp->joint_limit_flag(9] “ oootxlmotion_liniit_fUg(9j; 
teinp->ioint_limit_fUg(10] • coQrdjnotiaa_limit_fl^lO] 
tein|>-''joint_iiinit_fUg(l 1] • coord.motioii_liniit_flag[l 1] 
temp->joint_limit_flag(12j - Goardjnation_liniit_flag(12] 
teinp->joint_limit_fIag(13j • coord.niotioo_limit_flag(13] 
teTnp->jointJimit_flag[14)« coord.motioo_lifflit_flag[14} 
teinp-^oint_liniit_flag{13] coanLma(ion_limit_flag(lS] 
temp->joint_limit_fiag[I6] • ooard.motiao_limit_fUg{16] 

// leg_contact_flag 

letnp->leg_cootact_ilag{0] - coord.leg.suppott_flag(0]; 
temp->leg_contact_flag(l] - ooord.leg^suppait_flag(lj; 
tem{>->leg_coiilact_flag[2] => cooid.leg^sup(>ort_flag(2j; 
tetnp->leg_contact_ilag(3] - coord.leg^support_flag(3]; 
teinp->leg^contact_llagJ4] - coord.leg,siq)poit_ilag(4]; 
ten^>leg_contact_fUg(S] = coord.leg_suppott_flag[}j; 
tenip->leg_contact_flag[6] “ ooord.leg^support_flag(6j; 

return *temp; 
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} 

#ifiidefKINEMATICS H 
#defiiie KINEMATICS.H 

// 

// FILENAME: KwiwnitirrH 

#mdude "arcaatH'' 

^include "MabixMy.H" 
#indude*AbLeg.H' 
itmOaic 'AbBmfy.H* 

// stnicture deaigped to receive file in|)ut 
struct Pantng_Itemi { 
iialegDuni; 
ittbody. 
double dell; 
double del2; 
double del3; 
double del4; 
double del5; 
double del6; 

}; 


//structure desigoed to return the xyz coordinates the robot 
struct Retiiro_Co<xdinates ( 
double bod^[21]; 
double leglc(12]; 
double le^|l2]; 
double leg3c[12]; 
double Ieg4c[12i; 
double Ieg3c(12]; 
double ieg6c{12]; 
im tnotkn_limit_flag(18]; 
iot leg_su|i|>artJlag[6]; 
double aiigle[6][S]; 

}; 


// structure to recieve next robot motioiifioin gait plaimingalgoritfatns 

struct Next_Mati<n { 

public: 

double bodyiiiotioi)[6]; 
double leglmotioDp]; 
double leg2inoti<]a[3]; 
double legSmotionpj; 
double kgdmotionp]; 
double leg5iiiotioa(3j; 
double l^motioopj; 
hit leg,coiilact_fla^6]; 
im joim_liiiiit_fl^l8]; 
double iiibdJohit_ang|e[LEG][S]; 
double foot_l_coord[3]; 
double fi)ot_2_cootd[3]; 
double fix)t_3_ooatd{3]; 
douUe fi>ot_4_ooord[3]; 
double foot__S_ooord[3]; 
double fi>ot_6_cootd[3]; 
double body_oeiiter_oooid[6]; 

//constructor 
Next_MotiooO { 


bodyniotioo[0] > 0.0; bodyiiiotion[l] ~ 0.0; bodymotion[2] = 0.0; 
badyniation[3] = 0.0; bo<fyinotion[4] 0.0; bo<^inotioo[}] ^ 0.0; 
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kg_caaUct_fla((L£Gl] > 1; l«g_oaalact_flag[L£G3] ~ 1; 
leg;_ooaUGt_Sag[LEG3] > 1; kg;_oaB(act_flag(LEG4] > 1; 
lejL.caaiact_flag(LEGS] • 1; lf^ooBtact_fl*g(L£G6]« 1; 


teglmolioiipC] = 0.0; tegliiiotiaii{Y] > 0.0; leglinotiao(Z] 0.0; 
k^ZmotkiiiPC] • 0.0; le^2molicMi(Yl ° 0.0; “ 0.0; 

leg3motioa(X] > 0.0; teg3 i iiotiflo(Y] = 0.0; leg3iiiotioii[Z} - 0.0; 
IqptaKMiaapq > 0.0; leg4maliaii[Y] > 0.0; 1^4iDO(ioti[Z j 0.0; 
tegSmotioiipC]» 0.0; leg3mation|Y] ~ 0.0; legSiiiotiaa{Zj = 0.0; 
kgSmolionpC] > 0.0; leg|6ffloliaii(Y] ~ 0.0; legSmotiooIZ] - 0.0; 

joint_Iiiiiit_fUg(0]» 0; joint_liiiiit_flag(l] - 0; 
joiiit_ljinit_flag[2] - 0; joint_tiniit_flag{3] - 0; 
joiat_liiiiit_flag[4] - 0; joia_liiiiit_flag[5] - 0; 
joint_liinit_flag(6] - 0; joint_liinit_fUgI7] - 0; 
joint_liinit_flag(81 - 0; joim_lmiitjtUg{9] “ 0; 
joiiit_limit_fUg[10] - 0; joiot_liiBit_fUg{ll]« 0; 
joiat_liniit_fbg[12]« 0; joiitf_tiiait_flag(13] “ 0; 
joiat_liiiiit_fU8{14] > 0; joii]t_Ufflit_fUg(13j > 0; 
joint_]iinit_flag[16j >■ 0; joiiit_liinit_il«g(17]» 0; 


inixlJoint_ingle[L£Gl][CB] = (double)LEGl«60.0; 
iiibdJoint_aiigle[LEGl][HIP] ’°St«itTh^l; 
iiri)dJoii4_aiigle[LEGl][KNEEl]= StaitTheu2; 
■nbdJoiiit_tii8le[LEGl][KNE£2j> SUrtTheuG; 
iiibdJoiiit_*iigle[LEGl][FOOT] = 0.0; 

iEMJ<>iiit_«igle[LEG2][CB] > ((k>ub><!)LEG2*60.0; 
ndidjoint aiig]e(L£G2][HIPl ^StatTlieUl; 
ifibdJoat3*<>glc(LEG2j[KNEEl]- StattTheU2; 
BibdJ<>int_*iigle[LEG2](KNEE2]’= StaitTbeU3; 
iiibdJoiiit_«n8le{LEG2](FOOT]« 0.0; 

inbdJoint_*ngl*{LEG3][CB] - (double)LEG3*60.0; 
iiibdJofflt_<iig|e{LEG3][KIP] »SUitThkal; 
iididJoiit_aiigle[LEG3](KNEEl]> StutTbeu2; 
iabdJoint_<uigle[LEG3j[KNEE2j- StirtTbeta3; 
iiibdJoint_an8le{LEG3][FOOTl “ 0.0; 

inbdJoint_aiigle(LEG4](CB] - (double)LEG4*60.0; 
inb(lJoint_angle(L£G4][HI^ -SUrtTb^l; 
iiibdJoiiit_angle[LEG4][KNEEl]~ SUrtTheU2; 
iiibdJoiiit_angle[LEG4][KNEE2]>‘ Stiit'nieU3; 
inbdJoiiit_aii8le[LEG4][FOOT] = 0.0; 

iidKlJoiiit_angl«[LEGS](CB] > (double)LEGS*60.0; 
iiibdJoiiit_angle[LEG3][HIP] ^StaitTheUl; 
iiibdJoiiit_«ngle{L£G3][ICNEEl]= SttttTheU2; 
ii)bdJotiit_«ngle(LEGS][KNEE2j= SUrtThda3; 
inbdJoiiit_angle[LEG3j[FOOT] = 0.0; 

iiW>djoiiit_«ngIe(LEG6][CB] = (douUe)LEG6*60.0; 
iiibdJoiiit_aii^e[LEG(][HIF] ^StaitTh^l; 
iiibdJomt_aii^e[LEG6][KNEEl]> SUitTheU2; 
iiib<lJoint_«ngle[LEG6][KNEE2]> StartTheta3; 
inbdJoiiit_*ngle(LEG<][FOOTJ - 0.0; 

/* respect to WORLD Coonttnates */ 

foot_l_coonipCW]= 98.02; fbot_l eoorf[YW]= 0.0; foot_l_<»ordlZW]=0.0; 
foot_2_coonJ{XW]= 49.01; foot_2 eoonllYW]-84.88781; foot_2 eoort[ZW)=0.0; 
foot_3_coordpCW]=-49.01; foot_3~coordfYW]» 84.88781; foot_3 coofxi[ZW]=0.0; 
foot_4 coordpfW]=-98.02;foot 4 coordpTW]” 0.0; foot_4_co^{ZW]=0.0; 

foot 5“(»o«ipCW]-^9.01;foot“sIcooritYW]=-84.88781;foot_5 coord[ZW]=0.0; 
foot*6_coordpCW]= 49.01, foot"6_coord[YW]=-84.88781; foot_6 coord[ZWl=0.0; 


body_ceiiter_eoord[0] = 0.0; 






bo<i^_oa<er_ooanl[l] ~ 0.0; 
body_ocnlcr_ooanl{2] > 0.0; 
body_otnlcr_ooard{3] ~ 0.0; 
body_ccnter_oaatd[4] - 0.0; 
body ocolcr ooanl[S] > -TO.Tl; 

}; 

}; 

//stnictiite 

sinict WaUd’anmeter { 
public: 
ini phase; 
double atrkle; 
double directioa; 
douUetiipodiizr, 
double footbeigfal; 
double bodyapeed; 

WaDcFarameterO { 
phase >0; 

stride • Oe&ullSTRIDE; 

direction > 0.0; 

tripodsize = 98.02; 

footbeigfat - DefaultFOOTheight; 

bodyspeed >= (De£uiltSTRIDE/2.oyFINE; 

>; 

}; 

//atfudure 
struct Flag { 
public: 
mtphaaeEad; 
intgoal; 

//constructor 

FUeO{ 
phaseEnd-O; 
goal = 0; 

}; 

>; 

extern 

Retuni_Coofdinates FindPasitioas(A<iuarobolBody &, 

Ai]uaLeg &, AtpiaLeg &, Aq[uaLeg &, 
AquaLeg &, AquaLeg A, AquaLeg A); 


extern 

Pasting_Iteins File_Use(FILE *, AquarobotBody A, 

AquaLeg A, AquaLeg AquaLeg A, 
AquaLeg AquaLeg AquaLeg A); 


extern 

Next_Motion TransferToGait(Retum_Coordinates &, AquarobotBody A); 
#esxlif 

//FILENAME: LirdcO.C 

// PURPOSE: Declaratioos fiir class LinkO 

#iiiclude "LinkO.H” 

Linlc0::LinkD0: Lii* ( 0,37.5,0.0,0.0,0.0, -1.0, 

-360.0,360.0) 

{ 

node_list • new n]atrix(4,4,0.0); 
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node liit->va](03) ~1-; i>ode_liit>>v*J(l^)-1.; 
iiodeIli*t->viK2.0) -37.3; node_liit->veK 23 ) . 1 .; 

T_niatnx new inatnx(4,4,0.0X 

} 

//FILENAME: LiidiO.H 
//PURPOSE: Dec Ure Oo m for dag tinfcO 

MiSadetH USKO 
iMefineH_lJNKO 

#include 'Link.H' 

cUas UnkO : public Link 

{ 

private: 

public: 

LiidtOO; 

}; 

#cndif 

//FILENAME: Linkl.C 


kinchide*Liidcl.H’ 


// LinkI::LinklO: Link ( 0.20.0. -90.0, 66.4, 0.0,0,•100.0,73.4) 
// Changed by Keoji Suzuki A Chuck Sduie 
Linki::Linkl0: Link ( 0.20.0, -90.0.33.80,0.0,0,-100.0,73.4) 
{ 

node_Uit - new inatrix(4,4,0.0); 
node_liat->val(03) -L; 
node list->val(I,3) •!.; 
node~list->vaK2,0) - 20.0; 
node_list->val(23) - L; 

T_matrix = new niatrix(4,4,0.0); 


> 

//FILENAME: LinkLH 

// PURPOSE: Dedaiatioos for class LinkO 

#ifodefH_LINKl 

#de&ieH_UNKl 


^include "LialcH' 


class Linkl: public Link 

{ 

private: 

public: 

LinklO; 

>; 


#endif 
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// FILENAME: Lii*2.C 
//PURPOSE: DecUralioacfiirciaMLiidiO 


#inch«le'Liiik2.H* 

// linlr^ -IJnlr^O ; Lii* ( 0.50.0.0.0. -156.4.0.0.1.0. -156.4.23.6) 
// riiMged by Kcnji Suzuki & Chuck Schue 
linH -liiAlO : LinkC 0.50.0.0.0. -125.86.0.0.1.0. -156.4.23.6) 
{ 

nodeliit new matrix(4,4.0.0). 
iK)de_list->val(03) “L; node_li*>vil(13) “1.; 
iiode_U*t->v«K2.0) -50.; oode_li«->vil(23) - 1-; 

T_iiiatrix - new initin(4.4.0.0). 


} 

// FILENAME: Liiik2.H 

// PURPOSE: DecUntiau for cU« LinkO 

#ifoikfH_LINK2 
MeSae H_UNK3 

#inclu<le*LiidcH” 

clan Liiik2 : public Liidc 

{ 

private: 

public: 

Unk20: 

}; 

#«ndif 

//FILENAME: LinkS.C 

// PURPOSE: DeclaratioM for clan LinkO 


#iiiclude 'Link3.H’ 

Link3::Link30: Link ( 0.100.0.0.O.O.0,0.0.2.0. -360.0^60.0) 

{ 

node_list - new matrix(4.4.0.0); 
node_list->val(04) =L; node_li*t->val(I.3) =1.; 
node_list->val(2.0) -100.; node_list->val(2.3) - 1.; 

T_matrix - new matrix(4.4,0.0); 


} 

//«**••*«••••*•*•••**«*«•*•••*•*•••••< 

//FILENAME: Link3.H 

// PURPOSE: Declarationf for clan LinkO 

//*.....**•..**«•*....*«.*.».*•*****■ 

#ifedefH_LINK3 

#defineH_UNK3 

ilinclude''Link.H’ 
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cbMLink3 ; pid>lic Link 

{ 

phvate: 

public: 

Liak30; 

>; 


#aidif 

// *•••••**«••••••••••««••••*••••«••«•••••««*•••«•*•••«* 

//FILENAME: LitJcC 

./ PURPOSE: fanplcnienlatMn of d«a Ln^ 

// CONTAINS: UpdateAMatrix 0 
// Rotate (double angle) 

// RotateLink (double angle) 

jj ««««««•««•«*••«*«««•««««««•««•«•«««••««••««•«•««•««•• 


#mcliide'Link.H” 


const int Tnie1; 
const int False ~ 0; 

//FUNCTION: Link 
//PURPOSE: Condnictcr for Link 

Link::Link (int mIL double U. double ta, double ija, double ijd, double iL 
double min_ia, double max Ja) 

{ 

motion_limit_fUg > mlE 
liok_lenglh • 11; 
twist_ang|e • ta; 
inboardJoint_angle > ija; 
inboanl_j<Mni_displacemtot«ij^ 
inboard_link = il; 
minJoint_angle minja; 
maxJoint_angle = maxja; 

H_matrix => i>ewniatiix(4,4,0.0X 


H_matrix->UpdateTMatiix(ija,ta,U,ijd); 

) 

//FUNCTION;-Link 
// PURPOSE: destnicior for Link class 

Link::~LinkO 

{ 

delete T_matrix; 
delete node_list; 

} 

//FUNCTION: Rotate 

//PURPOSE; rotates a Liidc by changing the T Matrix 
// by the inboard j<»n angle desired 

//******«•*••*•*.«**•.»««•******•«.*.*.**..*«.«•***•«.*.. 

void Link::Rotate (matrix *mat, double angle) 

{ 

SetInboardJoiiitAngIe(angle); 

T_malnx->UpdateTMatiix(GetInboardJointAn^eO,GetTwistAn^eO, 

GetLinkLenglh0,GctInboardJointDi9lacement0X 

//the "mat” is the inboard link's A matrix (or the body's 
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//A oHltix for the aboard joint 
*H_aaliix - •mat • •T_mttix; 


} 


// FUNCTION: RotatcLak 

//PURPOSE: dctcrmtiws if the ratatioo ia withia physical 
// joint oaaatninls.Ifoulaide the woifcapaoe the min 
// armaxUniitappliGaUciauaed. 

// thiafuoclioDcaUalfaeRolalefoiictioa 


void Link::Roiaelink(niatrix *mni. double angle) 

{ 

double tcaler, 

teatcr - CelMinloiatAogleO; 

if (angle < teatcr) { 

angle • teatcr, 

SetMotionl jinitFlag(l); 

} 

teatcr - GetMaxIointAngleO; 

if (angle > teatcr) { 

angle • teatcr, 

SetMolicoLiniitFlag(lX 

} 

Rotale(mat, angleX 

} 


// FILENAME: Link.H 

// PURPOSE; Oeclaratiow for clan Liidc 

// COMMENTS: Definitian of Link claas 


MfodefH LINK 
MefineH.UNK 

#iiKtude <aliiio.h> 
Hinchide <inatli.ti> 
ttinciude *AbRigid.H* 
#include 'MatrixMy.H” 


clasa Link: public RigidBody 

{ 

private: 

iid niotion_Uinit_flag; 

double link_teoglh; 

double twiat_angle; 

double idboatdJoint_anglc', 

double inboard Joint_displaoenient; 

double inboard_link; 

double min Joint_angie; // rotary link 

double max joiin_angle; // rotary link 

public 

void RolateLiak(matrix*, double); 


matrix •T_nialrix, *node_litt, •H_mattix; 


Liiric (iid mlf double IL double ta, double ija, double ijd, double U, 
double min Ja, double max Ja X 


-Link(X 


bit GctMotionLnnitFlagO {return motion_lbnit_flag;} 
double GetUnkLenglhO {return Imkjenglh;} 
double CelTwhtAngleO {return twist_angle;} 
double GctlnboardJouitAngleO {return biboardJoiiit_angle;} 
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double GetlnboerilJointDiyUceniertO {tcbini ii)bowdJoiiit_dispUc«ment;} 
double GcUnboardlinlcO {retaun iiiboard_link;} 
double GetTMatrix(iiit row, iol col) (rctuni T_iiialrix->val(row,coI);} 
douUe GctMmlointAiigteO {return minJoiin_angle;} 
double GctMaxJointAngleO {return nuxJoint_angte-,} 


void SetMotiooLimitFUg(iin «) {niotian_limit_fUg = a;} 

void s»«t jnlct <»igrti(A»ihle a) {liiik_length == a;} 

void SetTwiitAogic(dauble a) {twiat_angle > a;} 

void SelInboanUondAngle(double a) {inboardJoiot_aiig|c = a;} 

void SetInboatdJaintDiaplaceman(double a) {inboafdJoint_di 9 Uoeniem =a;} 

void SetMioardLiidcCdouble a) {iiiboard_linlc a;} 

void SetTMaiiui(int row,iDt double a) 

{T_t*alrix->vaJ(row,col) “ a;} 
void SetMinJointAngJe(double a) {niinJoint_an£|e = a;} 
void Setb ^ax)oinLAngle(double a) {maxJomt_angle > a;} 

void RoUte(mattix*. double); 

}; 

#endif 

// FILENAME: MatiixMy.C 

// PURPOSE: Implementation of MatrixMy clan 

// CONTAINS: 

#include <stdio.b> 

^include <stdlib.h> 
itinclude <aliing.b> 

#ioclude <math.h> 
ttinclude "MalrixMy.H’ 

#include’AbLefrH'' 

matrix: anatrixO 

{ 

p • new matrep; 

p->r-4; 

p->c-4; 

p->m ” new double *[4]; 
intx; 

for (x “0; x<4; x++) 

p-^>m[x] • new double[41]; 

p.>n=- 1; 
intj; 

for (int i”0; i<4; i++) 

for(j”0;j<4;j++) 
p->m[i][j] = 0.0; 


matrix:3natiix(mtrows - 1, int col = 1, double initval = 0.) 

{ 

p = new matrep; 

p->r*rows; 

p->c ” col; 

p->m ~ new double *[rows]; 

iinx; 

for (x =0; x<rows; x-H-) 

p->m[x] = new double[col]; 

p->n= 1; 

intj; 

for (int i=0; i<rows; i++) 

for j-'-'-) 

p->m[i][)] = initval; 
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inathx: 3 iiatrix(coiift i>ialnx& x) 

{ 

x.p->irv+; 

P - x-p; 

} 

matrix matrix:'.opentorKoonst matrixft rval) 

{ 

if(-p->n“0) { 

^ (int x><l; x<row»0'. 

delete p->m[x]; 

delete p->m; 
delete p; 

} 

rvaLp->ii-t-+; 
p « rval.p; 
return 

} 


matrix;:-matiixO 

{ 

if(-p.>n = 0) { 

for (int x»0; x<ro\vsO; x++) 
delete p->m[x]; 
delete p*>m; 
delete p; 

} 

} 

double & matrix; :val(iiit row, iot col) const 

{ 

return (p->m(tow][col]); 

} 

matrix matrix::operator*(const roatrix& arg) 

{ 

matrix resutt(rowsO,arg.colsO.O.O); 
for (bit row=0; row<rowsO’.row++) { 
intooU 

for (col=0; col<arg.colsO; co1-h-) { 
double sum=0.0; 
for (bit i=0; i<colsO; i++) 
sum += p->m[row][i] • arg.val(i,col); 
result val(row,col) = sum; 

} 

} 

return result; 

} 

matrix matrix; :operator*(double a) 

{ 

matrix result(rowsO,colsO,0.0); 
for (bb i“0; i<rowsO; i+-^) { 

for (int j»0; j<cols0; j++) { 
double ans; 

ans = result val(ij) • a; 
result va](ij) = ans; 

} 

} 

return result; 

} 

matrix matrix: :operatorKconst matrix& arg) 

{ 

matrix sum(rowsO,colsO,0.0); 
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for (int i-0; i<rowiO; i++) { 
iatj; 

for (j”0; j<col*0; j-t-+) 

sum.p->ni[i][j] “ p->in[i][j] + ifK.val(tJ); 

} 

return sum; 

} 

void matrixi.'priidO 

{ 

for (int row«0; row<row(sO; row++) { 
int col; 

for (col=0; col<cols(X tol+ i-) 

prinlf(*%6.6f", ivO>ni(row][coll); 
printf("\n">. 


matrix & matrix::HomogeneousTnns£iirm(double azimuth, double elevation, 
double roll, 

double X, double y, double z) 

{ 

double spsi = sin(azimuth); 

double qni ■ cos(azinuith); 

double sth « sin(elevatioo); 

double cth > cos(elevation); 

double sphi ~ sia(roU); 

double q>hi = cos(toU); 

val(0,0) ” (qni • cthX 

val(0,l) = ((qiti * sth * sphi) • (spsi * q>hi)); 

val(0,2) = ((qisi • sth • cphi) + (spsi • qihi)); 

val(03)“x; 

val(l,0) = (spsi • cth); 

val(l,l) “ ((cpsi * cphi) + (spsi • sth • sphi)); 

val(l,2) »llspsi * sth • cphi) - (cpsi * sphi)); 

val(U)“y, 

val(2,0)»(-sth); 

val(2,l) = (cth * sphi); 

val(24) = (cth‘cphi); 

val(2,3)-z; 

val(3,0) = 0.0; 

val(3,l) = 0.0; 

vaI(3,2)-0.0; 

val(3,3) = 1.0; 

return *this; 


matrix & matrix: :DHMatrix(double cosrotate, double sinrotate, 
double costwist, double sintwist, 
double length, double translate) 

{ 

val(0,0) = cosrotate; 

val(0,l) = -1 * sinrotate; 

val(0,2) = 0.0; 

val(0,3) = length; 

va](l,0) = sinrotate • costwist; 

val(l,l) = costwist * cosrotate; 

val(l,2) = -1 • sintwist; 

va](l,3) = translate * -1 * sintwist; 

va](2,0) = sintwist * sinrotate; 

va](2,l) sintwist * cosrotate; 

va](2,2) “ costwist; 

val(2,3) = translate • costwist; 

val(3,3)= 1.0; 
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rdurn *liiis; 

> 

Matrix ft ■iiatrix::UpdateTMririx(douUe a, douUc b,doubU c, double d) 

{ 

a » a • deg_to_nd; 
b “ b • deg_to_rMl; 
double oovotate <■ ooe(aX 
double sinrotale - stii(a); 
double ooatwist > coc^); 
douUe liiitwist siii(bX 

DHMatrix(co«oUte, siiirolate, coctwixt, tintwiit,c,d); 


fetuni*lliis; 

} 

matrix ft iiiatrix::TraiiafoniiNodeLiit(aiatrixftH matrix, matrix ftb) 

{ 

matrix tem|i(4,1,0.0); 

foe (int i “ 0; i<).rovnO; i++) { 

tefiip.val(0,0) = b.val(i,0); 
temp.val(l,0) * b.val(i,l); 
temp.val(2,0) • b.val(i,2); 
temp.val^,0) ” b.val(i3); 
matrix middle ° H_matrix * leap; 


val(i,0) - middle.val(0,0); 
va](i,l) - middle.val(l,0); 
val(i,2) - middle.val(2,0); 
val(i3) = middle. val(3,0); 
}; 


retuni*this; 

} 

matrix &tOMtnx::Truisf<xmBodyLisl(autrix&H aatzix, aathx &b) 

{ 

matrix tcmp(4,l,0.0); 

for (iid i=0; i<).roMwO; { 

temp'.\«l(0,0) = b.^(i,0); 
teii9.val(l,0) = b.val(i,l); 
tenap.val(2,0) « b.val(i,2X 
temp.val^,0) = b.val(i,3); 
matrix middle - H_mxtrix * temp; 


va](i,0) = middle. val(0,0); 
val(i,l) = middle. val(l,0); 
val(i4) = middle. val(2,0); 
val(i,3) = middle. valp,0); 

}; 

return *tliis; 

} 

//M*....********...*.***********.,****.*.*.......**, 

// FILENAME: MatrixMy.H 

//PURPOSE; To provide for a matrix class to acGonqiluli 
// some necessary robotic and kinematic needs. 

// COMMENTS: DHMatrix, Homogeneous Transform, and 
// TransformNodeUst ate included 

//e*..*.*********..*****.**..*****..*...***..**.*.... 

#ifiidefH_MATRIX 

#de&ieH_MATRlX 


const double deg_to_rad = .017453292519943295; 
class matrix { 
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Unict matrap { 

>V. 

public: 

iiiitnx(ooiist matrixft x); //copy initializer 

-matrixO; 

maliixO; 

malrix(int, int, double ); 

mabix operator>=(coost matiixft rval); 

matrix operatorKconat matrixdl rval); 

matrix operator*(coait matrixft rval); 

matrix operator*(double); 

double ft val(iiit row, int ool)ooiiit; 

voidprintO; 

int rmwtO oonit {return p->r,}; 
int oolaO (return p->c;}; 

matrix ft HamogeneouaTrat«ifiitni(double,double,double,double,double,double); 

matrix ft DIlMatrix(double, douU^ double,doubie, double, double); 

matrix ft l^)dateTMatrix(d^te, d^te, double, double); 

matrix ft TtaiisfomiNodeLiat(malrixft, matrixft); 

matrix ft TianafijtmBodyf iri(inatrixft, ituuixft); 

}; 


#endif 
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APPENDIX D: 


AQUAROBOT SIMULATOR USER’S MANUAL 


This appendix contains the information necessary to use the version of the 
AquaRobot Simulator working at the time this thesis was written. The User’s Guide was 
intended as a stand alone document; it may be removed from the thesis and used 
separately. 
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AQUAROBOT SIMULATOR 
USER’S GUIDE 
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PREFACE 


PURPOSE OF THE MANUAL 

This manual provides user-specific information necessary to operate the 
AquaRobot Simulator on the Silicon Graphics Personal Iris. Code modification details are 
purposely excluded. 

KNOWLEDGE LEVEL 

Some knowledge of the Silicon Graphics workstation, the UNIX operating system, 
and the mouse pointing device is presumed. 

CONTENTS 

l. Logging On 
n. Compiling 

m. Using the Simulator 

Attachment A; Flowchart and Function Descriptions 
Attahcment B; Required Files Listing 
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L LOGGING ON 


Step 1; At the Login screen, type in the appropriate responses to the Login prompts. 

Login, aquarobo 
Password . _ 

The password is available from Dr. Kanayama or Dr. McGhee. Once Login 
is completed, the UNIX operating system is loaded. 

Step 2: Click on the CONSOLE icon and size the CONSOLE window as desired. 

Step 3 . Press enter to get to the command prompt. 

Step 4; Use the change directory command (cd) to change to the directory shown. 
WorkSpace/thesis/arsim 

Step 5: Check to ensure you are in the correct directory using the present work directory 
command (pwd). The correct directory is as shown. 

/n/auvsim4At>ork/aquarobo/WorkSpace/thesis/arsim 

Step 6: You have now completed the Login process and are positioned in the correct 
working directory. 


n. COMPILING 

Step 7; To compile this version program, type in the make command {make) and press 
enter. The make command has executed properly when the command prompt returns. 
Once the command prompt returns, the program has compiled correctly. 

m. USING THE SIMULATOR 

Step 8; At the command prompt, type aqua to start the AquaRobot Simulator. Some 
interaction between the user and the simulator is now required. When prompted, 
determine whether you want to see the discrete (type in 0) or continuous (type in 1) 
alternating tripod gaits. Then determine the desired goal point and enter its x-coordinate, 
space, and y-coordinate. The positive x-axis lies lengthwise to the right on the screen and 
the positive y-axis is outward towards the viewer. Next, you may be prompted to select 
the footpad size for the simulation. Choosing zero (0) selects the 25 centimeter pootpad; 
choosing one (1) selects the 45 centimeter footpad. The CONSOLE window shows 
program status and the AQUAROBOT window shows the graphic output. 


242 






Step 9; Four viewpoints are available to the user as a right mouse button menu; Camera: 
ABOVE, Camera; LEG5-6 VIEW, Camera; LEGl VIEW, and Camera; LEG4 VIEW. To 
change the viewpoint from the default Camera: LEG2-3 VIEW view, quickly press the 
right mouse button afrer typing aqua and moving the mouse pointer into the 
AQUAROBOT window. 

NOTE; The menu selections FBLEREAD, RESETFILE, and KEYBDREAD 
should not be used. 

Step 10; The RESET menu selection places AquaRobot in the default LEG2-3 
VIEW. 

Step 11: To repeat the aqua program, with the default Camera. LEG2-3 VIEW 

viewpoint, move the mouse pointer into the CONSOLE window and choose one of the 
following methods; 

a. type aqua 

b. press enter 

type a (this is the history/repeat sequence) 
press enter 

Step 12: To exit the aqua program, press the right mouse menu button and select 

Exit from the choices listed. 

Step 13: To exit the CONSOLE window, use the right mouse button to get Log 

Out; then answer "yes". 
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ATTACHMENT A: FLOWCHART and FUNCTION DESCRIPTIONS 
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The simulator files are separated into four modules. The files that comprise the 
Makefile, Graphics, and Matrix Manipulation Modules are only cursorily addressed here. 
The Gait Planning Module is described in more detail. 

1. Makefile 

The Makefile allows the make utility to intelligently compile the program. This 
file includes instructions for how and when to compile the va'^'ous files that comprise the 
AquaRobot Simulator. The AquaRobot simulation program uses the UNIX make utility to 
link together the 27 individual files with the graphics, math, and standard input-output 
libraries. In this case, the Makefile generates the executable program aqua. 

2. Graphics 

The 3D stick-figure graphics code was originally written by Sandra Davidson 
(pAV93]. The graphics files (code modules) are included in Appendix C for easier 
reference. Some file names were changed to avoid software configuration management 
problems. Additionally, some graphics code was modified because of the requirements of 
the gait planning code. For example, the FARCLIPPING value and the CAMERA viewing 
angles were changed in the bot.H file and the CB height was changed in the AbBody.C 
file. Because the Graphics Module polls the Gait Planning Module, a suitable interface 
between the two was constructed in the Kmematics.H and Kine,matics.C files. The 
graphics code consists of the following files: 

♦ AbBody.H and AbBocfy. C; 

♦ A bLeg.H and AbLeg. C, 

♦ AbRigidH and AbRigid C, 

♦ bot.H and bot.C; 

♦ Kmematics.H and Kinematics. C; 
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♦ Link.H and Link. C, 

♦ LinkO.H and LinkO. C; 

♦ Linkl.H and Linkl. C, 

♦ Link2.H and Link!. C, and 

♦ Links.H and Links. C. 

3. Matrix Manipulation 

The matrix manipulation code was also originally written by Sandra Davidson 
[DAV93]. It provides 4x4 matrix multiplication capability. This code is an essential part 
of the graphics code, although it is not used in the gait planning code. The Matrix 
Manipulation Module includes the following files, found in Appendix C: 

♦ Matrixkfy.H and MatrixMy. C. 

4. Gait Planning 

The Gait Planning Module (GPM) consists of the following files; 

♦ arconst.H; 

♦ cafunc.H and cafunc. C, and 

♦ artpgaitH and artpgait. C. 

To support easier reference and maintenance, most constants are grouped together into 
the file arconst.H (aquarobot constants). 

The arfunc.H/C (aquarobot functions) files contain 13 functions. The min and 
max functions simply return the minimum or maximum of two expressions, respectively. 
The ellipse function calculates the incremental foot trajectory along an elliptical path 
between the current foot position and the desired foot position. The kinematics function 
performs kinematics for the Gait Planning Module. Kinematics for the Graphics Module 
are performed in the Graphics Kinematics.C/H files. The inv kinematics function 
performs the inverse kinematics operations. The world bocfy function provides coordinate 
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transformation from the body-fixed coordinate system to the world coordinate system. 
The bocfy_\vorld function provides coordinate transformation from the world coordinate 
system to the body-fixed coordinate system. 

The maxdistance_25cm function determines the maximum stride for the 
workspace defined by the 25 centimeter footpad. The maxdistance_45cm function 
determines the maximum stride for the workspace defined by the 45 centuneter footpad. 
The arcint function calculates the intersection of a directed line and an arc segment. The 
segint function calculates the intersection of a directed line and a line segment. The stable 
function calculates the stability of a tripod. The staparam function determines the 
longitudinal stability margin and the x and y intercepts of the directional ray originating at 
the CB and terminating at the point of intersection on the selected tripod. 

The artpgait.H/C (aquarobot tripod gait) files 15 functions. The get_goal 
function allows the user to input the desired goal point to which the AquaRobot simulation 
walks. The getjnem function alh vs the user to choose between the DATG or the 
CATG. The getJootchoice function allows the user to determine whether the 25 
centimeter or 45 centimeter footpad is used. The gait algorithm function executes either 
the DATG or the CATG gait algorithms, as previously determined by the user. The 
robotjnodel function initializes the joint angles for the simulation. 

The disc set Jlag function initializei> the flags used during the DATG algorithm. 
The disc^aitplanning function determines the walking parameters, such as stride length 
and direction, used during the DATG algorithm. The discjripodjnotion function 
calculates and executes incremental joint motions necessary to move the AquaRobot 
simulation using the DATG. 

The cont set Jlag function initializes the flags used during the CATG algorithm. 
The cont^ait jplanning function determines the walking parameters, such as stride length 
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and direction, used during the CATG algorithm. The contjripod motion function 
calculates and executes the incremental joint motions necessary to move the AquaRobot 
simulation using the CATG. 

Some functions in the artpgaitC file are used only for printing out the status of 
joints, feet, etc. These functions include print status, print walkpara, print_gnu_data, 
KnA printJointdata. 
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ATTACHMENT B: REQUIRED FH^ES LISTING 





Makefile 

Makefile 

Graphics 

AbBodyH 

AbBody.C 

AbLegH 

AbLeg.C 

AbRigidH 

AbRigid.C 

bot.H 

bot.C 

Kinematics.H 

Kinematics.C 

Link.H 

Link.C 

LinkOH 

LinkO.C 

LinkLH 

Linkl.C 

Link2.H 

Link2.C 

Links H 

Links.C 

Matrix Manipulation 

Matrb^y.C 

MatrixMy.H 

Gait Planning 
arconst.H 
arfunc.H 
arfunc.C 
artpgait.H 
artpgait.C 
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